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本 书 由 澳大利亚 墨尔本 皇家 理工 大 学 的 Reza N. Jazar 所 著 ， 是 原 书 的 
第 2 版 。 全 书 分 为 运动 学 、 动 力学 和 控制 技术 3 个 部 分 ， 共 15 章 ， 主 要 
内 容 包括 : 概述 、 旋 转运 动 学 、 定 向 运动 学 、 运 动 的 运动 学 、 正 向 运动 
学 、 首 向 运动 学 、 角 速度 、 速 度 运动 学 、 运 动 学 中 的 数值 法 、 加 速度 运动 
学 、 运 动 动力 学 、 机 器 人 动力 学 、 路 径 规 划 、 时 间 最 优 控制 、 控 制 技术 。 

本 书 可 作为 机 械 电 子 工程 、 电 气 工程 、 电 子 工程 、 自 动 控制 等 专业 的 
高 年 级 本 科 生 、 工 程 硕士 生 或 博士 生 以 及 理工 科大 学 教师 的 教学 参考 书 ， 
也 可 供 从 事 机 器 人 和 自动 化 设备 或 生产 线 等 应 用 开发 工作 的 研发 人 员 或 相 
关 工 程 技术 人 员 学 习 和 人 参考。 
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Reza N. Jazar 教授 简介 























的 一 些 大 学 工作 。 











Reza N. Jazar 系 澳大利亚 墨尔本 皇家 理工 大 学 航空 、 机 械 与 制 
造 工程 学 院 机 械 工程 系 教授 ，1990 年 取得 德黑兰 理工 大 学 硕士 学 
位 ， 研 究 方向 为 机 器 人 学 。1997 年 ， 获 得 伊朗 沙 力 夫 理工 大 学 博士 
学 位 ， 研 究 方向 是 非 线性 动力 学 和 应 用 数学 。1999 年 ， 开 始 在 北美 








Reza N. Jazar 教授 是 经 典 动 力学 和 非 线性 动力 学 方向 的 专家 ， 
他 在 动力 学 建 模 和 数学 建 模 方向 有 着 丰富 的 经 验 。Reza N. Jazar 在 
很 多 大 学 工作 过 ， 他 利用 自己 的 工作 经 验 ， 提 出 了 许多 理论 和 新 思 





想 ， 并 且 在 经 典 动 力学 、 机 器 人 学 、 控 制 和 非 线性 振动 等 方面 也 有 很 多 的 发 现 。Razi GY 
Fr) 加 速度 、 时 间 导 数理 论 、 无 秩序 变换 、 连 铸 机 理论 、 汽 车 驱动 器 算法 、 浮 动 时 间 法 、 能 








量 释放 率 理论 、RMS 最 优 法 ， 这些 都 是 他 的 发 现 和 新 思想 中 的 部 分 内 容 。 











Reza N. Jazar 教授 发 表 学 术 论 文 、 技 术 报 告 200 余 篇 ， 出 版 科技 书籍 30 余 本 ， 其 中 包 


括 《 机 器 人 学 应 用 理论 》《 汽 车 动力 学 : 理论 与 应 用 》。 
他 目前 的 研究 方向 是 非 线 性 振动 与 非 线 性 动力 学 ,包括 汽车 非 线 性 振动 与 非 线 怕 
学 、 机 器 人 学 、MEMS、 轨 道 力学 和 机 电 一 体 化 等 。 





动力 


译 者 序 





当前 ， 机 器 人 正在 不 断 地 替代 人 类 完成 繁重 的 体力 劳动 、 部 分 脑力 劳动 以 及 在 恶劣 环境 

















的 工作 ， 机 器 人 正在 成 为 现代 工业 中 不 可 缺少 的 组 成 部 分 ， 
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HERBER., BL tit 
、 电 子 、 自 动 控制 、 计 算 机 、 人 工 智能 等 学 科 领 域 中 的 相关 知识 和 技术 而 形 























商用 机 器 人 最 早 于 1961 年 由 美国 Unimate 公司 正式 生产 ， 随 后 各 种 商用 机 器 人 快速 发 
展 ， 机 器 人 理论 也 应 运 而 生 。 

本 书 由 澳大利亚 墨尔本 皇家 理工 大 学 的 Reza N. Jazar 教授 所 著 ， 从 理论 及 应 用 的 角度 
出 发 ， 阐 述 机 器 人 学 应 用 理论 。 第 1 版 由 Reza N. Jazar 教授 于 2007 年 在 美国 曼哈顿 大 学 机 
械 工 程 系 任教 期 间 出 版 ， 第 2 版 于 2010 年 出 版 。 本 书 是 Reza N. Jazar 教授 在 机 器 人 学 应 用 

















方面 的 多 年 教学 和 科研 工作 积累 的 成 果 ， 是 他 对 机 器 人 学 应 ) 




















体现 。Reza N. Jazar 教授 依据 机 器 人 学 


到 





的 主要 构成 和 特点 ， 从 理论 应 用 层面 将 








部 分 : 运动 学 、 动 力学 和 控制 技术 。 本 
时 总 结 了 每 章 的 主要 内 容 ， 说 明了 使 
练 ， 内 容 翔 实 ， 深 入 浅 出 ， 便 于 读者 学 习 和 
治学 的 学 者 风范 和 教书 育 人 的 深厚 情 ; 
者 学 习 、 探 讨 、 研 究 、 应 用 和 参考 。 
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4:45 Aw. EME, RIER, BEM. ROPE NE. Jaw, 
第 1 一 第 9 章 和 全 书 习 题 由 周 高 
第 13 一 第 15 SH ee 
电学 院 尚 会 超 博士 、 李 上 峥 峰 博 士 、 席 建 普 博士 、 牛 继 广 
ETME. 

在 此 期 间 ， 非 常 感谢 中 原 工 学 院 机 电学 院 赵 则 祥 教 授 、 郑 州 大 学 信 
翻译 稿件 所 提出 的 








工 如 下 : 前 言 、 
2 章 由 朱 强 翻译 ， 


书 统 稿 、 整 理 和 校对 工作 。 具 体 分 
第 10、 第 11 章 由 郭 士 锐 翻译 ,第 1 
赵 则 祥 翻 译 。 同 时 ， 中 原 工 学 院 机 


士 及 高 级 实验 师 贺 红 勋 和 张 素 香 也 参与 了 部 分 章节 的 整理 
























































教授 、 郑 州 大 学 机 械 工程 学 院 赵 风 霞 教授 对 本 
要 感谢 机 械 工业 出 版 社 对 本 书 的 出 版 所 做 的 一 切 
学 院 对 译 者 的 支持 和 指导 。 

本 书 可 作为 机 械 电 子 工程 、 电 气 工程 、 电 子 工程 、 








[ 作 和 努力 ， 















































不 。 本 书 不 愧 是 机 器 人 领域 中 的 


自动 控 





对 重要 的 内 容 、 定 理 或 结论 进行 了 解释 和 记 
] 符 号。 全 书 布局 合理 ， 逻 辑 性 强 ， 论 述 严谨 ,语言 精 
握 ， 它 充分 体现 出 了 Reza N. Jazar 教授 严 
籍 ， 值 得 广大 读 
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宝贵 意见 和 
最 后 要 特别 感 鹿 


E 论 思考 、 总 结 和 研究 的 集 ! 
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息 工 程 学 院 王 延 年 副 
NIM. Talay te 
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时 中 原 工 学 院 机 
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程 硕 士 生 或 博士 生 以 及 理工 科大 学 教师 的 教学 参考 书 ， 也 可 化 


TE 




















REMH 











F 发 工作 的 研发 人 员 或 相关 工程 技术 人 员 学 习 和 参考 。 








由 于 译 者 水 了 
者 、 专 家 和 学 者 








t 评 指正 ， 意 见 和 建议 反馈 邮箱 : 





郑州 弘 德 苑 


FE 级 本 科 生 、 
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和 机 器 人 和 自动 化 设备 或 生 


FF 有 限 ， 加 之 时 间 人 仓促， 书 中 难免 有 不 妥 之 处 、 缺 陷 甚 至 错误 ， 有 奶 请 广大 读 
zhougf123456@sina.com, 


周 高 峰 





如 果 没 有 来 自我 的 学 生 们 ， 特 别 是 来 自 哥 伦比 亚 大 学 的 我 的 那些 学 生 们 的 评论 和 建议 的 
话 ， 这 本 书 将 不 可 能 有 第 2 版 。 在 本 书 所 介绍 的 新 内 容 中 ， 很 多 都 是 学 生 反 馈 的 直接 结果 ， 
是 他 们 帮助 我 提炼 和 整理 了 材料 。 

我 写 这 本 书 的 目的 就 是 为 学 生 提 供 能 够 用 得 上 的 知识 。 幸 和 运 的 是 ， 我 已 经 成 功 地 编写 出 
了 能 够 涵盖 机 器 人 学 中 包括 细节 和 解释 在 内 所 有 方面 的 一 本 参考 书 。 

这 本 书 的 第 1 版 是 在 2007 年 出 版 的 。 之 后 不 久 ， 它 就 成 了 机 器 人 领域 中 广 受 欢迎 的 参 
考 书 。 我 很 想 感谢 已 经 使 用 过 或 者 参考 过 本 书 的 学 生 和 老师 们 。 是 你 们 的 问题 、 评 论 和 建议 
帮助 我 创作 了 本 书 的 第 2 版 。 

本 书 沿 用 第 1 版 的 符号 体系 。 


Cv) 


原 书 第 1 版 前 
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本 书 作为 课本 是 为 工程 学 专业 的 学 生 而 设计 的 ， 它 介绍 了 有 关机 器 人 学 中 的 基础 知识 。 
的 运动 学 、 动 力学 和 控制 开发 计算 机 程序 。 








利用 这 些 基 础 知识 能 够 为 分 析 机 器 人 系统 ! 
自从 1970 年 以 来 ， 随 着 机 器 人 领域 的 飞速 发 
然而 ， 该 主题 与 经 典 认 知 有 关 ， 在 现代 发 


























涵盖 机 器 人 学 ! 





























解析 统一 性 的 发 展 ， 对 新 书 的 需求 日 益 上 升 。 


机 右 人 的 古典 运动 学 和 动力 学 起 源 于 过 去 4 个 世纪 中 伟大 科学 家 们 的 研究 工作 ， 他 们 建 
立 了 方法 学 和 动态 系统 行为 的 认识 学 。 自 从 20 世纪 初期 以 来 ， 动 态 科 学 的 发 展 已 经 转向 了 
可 控 的 人 工 系 统 分 析 。 因 此 ， 对 于 机 器 人 的 分 析 来 说 ， 人 们 期 望 发 展 的 就 是 融合 含有 控制 理 





论 的 运动 学 和 动力 学 。 
其 他 的 重要 发 展 就 是 精确 
本 书 层级 














本 书 是 由 在 非 线性 动态 系统 中 的 近 几 十 年 的 研究 发 














E, Ki Al 
展 中 仍然 保持 着 热度 。 事 实证 明 没 有 哪 一 本 书 能 够 
几乎 所 有 的 主题 。 由 于 机 器 人 领域 的 持续 发 展 ， 以 及 运动 学 、 动 力学 和 控 甫 








出 现 了 机 咒 人 学 这 个 主题 。 
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的 本 科 生 、 研 究 生 课程 使 


























J]; SAFES 
授 的 就 是 机 械 系统 中 有 关 空 间 运 动 学 和 动力 学 的 主题 ， 





快速 的 数字 计算 以 





























展演 变 而 来 的 ， 可 供 有 关机 器 人 学 ， 


FE 级 的 本 科 生 和 一 年 级 的 工程 研究 生 。 该 书 最 初 讲 




















因此 ， 它 提供 的 是 有 关机 器 人 运动 学 


和 动力 学 的 基础 知识 和 前 沿 主 题 。 整 本 书 涵盖 在 两 次 连续 的 课程 中 ; 然而 ， 跳 过 一 些 课 程 ， 
在 一 门 课程 中 集中 学 习 本 书 也 是 可 以 的 。 本 书 要 求学 生 具 有 运动 学 、 动 力学 以 及 数值 方法 的 











一 些 基础 知识 。 















































本 书 的 内 容 是 处 于 应 用 理论 层级 。 它 深度 地 解释 了 许多 概念 ， 并且 强 调 了 这 些 概念 的 应 


用 ,同时 解释 和 证 明了 大 部 分 相关 更 








定 的 主题 都 是 机 器 人 领域 中 特别 感 兴 


题 和 方法 介绍 给 学 生 。 
本 书 组 织 架构 











组 织 本 书 是 为 了 便于 教师 教学 和 学 生 自 学 。 
第 1 章 “ 概 述 ”， 回 顾 了 机 器 人 基础 ， 包 括 机 器 人 的 分 类 和 发 展 历史 。 


第 上 部 分 “运动 学 ” 











E 论 。 在 整 本 书 中 ， 特 别 强 调 引 入 概念 的 物理 意义 。 所 选 
趣 的 主题 。 这 样 做 的 目的 就 是 将 机 器 人 领域 中 大 量 的 主 





陈述 了 机 器 人 的 正 向 运动 学 和 道 向 运动 党。 运动 学 分 析 指 机 器 人 在 


关节 与 基 座 坐标 系 空间 中 的 位 置 、 速 度 和 加 速度 分 析 。 在 末端 执行 器 和 关节 变量 之 间 建 立 运 




















BARA. IAT 























杆 ， 以 便 对 复杂 机 器 人 进行 正 向 运动 学 处 理 。 











于 描述 刚体 坐标 系 的 Denavit-Hartenberg (D-H) 方法 ， 并 利 
进行 正 向 运动 分 析 。 机 右 人 的 模块 化 处 理 概念 可 以 很 好 地 向 我 们 展示 如 何 综合 分 析 简 单 碧 
对 于 逆向 运动 学 分 析 ， 本 书 介 绍 了 耦合 思想 、 

















] 该 方法 











逆 和 矩阵 方法 和 和 迭代 技术 。 很 明显 ， 球 形 机 械 手 的 表述 就 需要 我 们 将 分 析 法 应 用 于 逆向 运动 学 





之 中 。 
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第 开 部 分 “动力 学 ”深入 陈述 和 讨论 了 机 器 人 动力 学 。 这 样 做 的 一 个 目的 就 是 回顾 基本 
的 方法 ， 并 说 明 这 些 方法 怎样 用 于 前 面 章 节 中 提 到 的 机 器 人 运动 学 中 的 位 移 问题 。 此 部 分 还 
介绍 了 递归 的 牛顿 - 欧 拉 (Newton-Euler) 动力 学 、 拉 格 朗 日 (Lagrangian) PRA. BLA 
惯性 和 矩阵 和 广义 力 等 概念 ， 并 将 这 些 概念 运用 到 运动 的 动态 方程 推导 之 中 。 

第 轩 部 分 “控制 技术 ”说 明了 用 于 机 器 人 的 最 优 时间 控 制 的 上 浮 时 间 技 术 。 这 项 技术 可 
应 用 于 开 环 控制 算法 之 中 。 此 部 分 还 介绍 了 计算 转 矩 的 方法 ， 在 此 过 程 中 可 利用 前 馈 和 反馈 
信号 的 合成 确定 系统 误差 动力 学 。 

叙述 方法 

本 书 的 叙述 方法 是 “事实 一 原因 一 应 用 ”。“ 事 实 ” 就 是 我 们 在 每 节 中 介绍 的 主题 。“ 原 
因 ” 就 是 对 事实 进行 “证 明 ”。 最 后 将 事实 “应 用 ”在 一 些 例子 中 进行 检验 。 相 关 实 例 是 本 
书 中 非常 重要 的 一 部 分 ， 因 为 它们 能 够 说 明 怎 样 应 用 在 “事实 ”中 所 介绍 的 知识 。 它 们 也 包 
含 了 一 些 需要 扩展 主题 的 其 他 “事实 ”。 

前 提 条 件 
由 于 本 书 主要 是 为 高 年 级 本 科 生 和 一 年 级 工程 专业 的 研究 生 而 编写 的 ， 因 此 有 一 个 前 提 
就 是 用 户 要 熟悉 矩阵 代数 和 基本 的 反馈 控制 。 对 于 本 书 的 读者 而 言 ， 前 提 条 件 就 是 读者 要 有 
运动 学 、 动 力学 、 矢 量 分 析 和 移 阵 论 的 基础 。 在 本 科 前 三 年 ， 通 常 讲授 这 些 基础 知识 。 

单位 系统 

本 书 所 采用 的 单位 系统 是 国际 单位 系统 CSD. ， 除 非 另 有 说 明 。 度 C 或 者 弧度 (Crad) 
单位 被 用 来 表述 角度 变量 的 数量 。 

符号 

1) 小 写 粗 黑 和 斜体 字母 用 来 表示 矢量 。 矢 量 在 ” 维 欧 几 里 得 空间 中 表述 。 例 如 : 







































































r, s, d, a, by c, 
P. Gy. Vi Wr Yn &\ 
o a. 8, 0. 6. © 
2) 大 写 粗 黑 斜 体 字母 用 来 表示 动态 矢量 或 者 动态 和 矩阵 。 例 如: 





F. M J 
3) 带 “” 的 小 写字 母 用 来 表示 单位 矢量 。 单 位 矢量 不 加 粗 。 例 如 : 
Ty, Je Ky ed ees €p 
D 带 “~” 的 小 写字 母 表示 一 个 与 一 矢量 相关 的 3X3 斜 对 称 和 矩阵 。 例 如 : 



































0 — a3 a2 ay 
a= a3 0 “ay d= d3 
a» dı 0 as 


ol 


) 两 个 字母 上 方 的 箭头 表示 起 点 和 终点 的 位 置 矢 量 。 例 如 : 
ON 表示 一 个 从 O 点 到 N 点 的 位 置 矢量 。 
) 小 写字 母 上 的 一 个 双 箭头 表示 与 四 元 数 相 联 系 的 4X4 和 矩阵。 例如; 


D 


和 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





do ~ Gi ”qq qs 
ty qı qo U3 q2 ~ z a 
q= g=qotqiitg: j task 
q2 q3 qo di 
q3 ~q? qı qo 





7) 非 粗 体 小 写字 母 表示 矢 量 的 长 度 。 例 如 : 





r=|r|, a=|a|, b= 


|b], s=|s| 


8) 大 写 黑体 字母 A4、Q、R 和 TT 表示 旋转 矩阵 或 变换 矩阵 。 例 如: 











ca 0 一 Sa 一 1 
ca sa 0 
Q ö op 0 1 0 0.5 
= ú ， ,一 
° sa 0 ca 0.2 
0 0 1 
0 0 0 1 
9 大 写字 母 B 用 来 表示 刚体 坐标 系 。 例 如 : 
BCozyz)、 BCOzyz)、 Bi(o1zT1y1z1) 


10) 


11) 


大 写字 母 G 用 来 表示 一 个 全 局 的 惯性 的 或 者 固定 的 坐标 系 。 例 如 : 


变换 入 


G、GCXYZ)、G 
E 阵 右 下 标 表示 参考 坐标 系 。 例 如 : 





(OXYZ) 


Ts 


表示 参考 坐标 系 B(ozyz) 的 变换 矩阵 





12) 变换 矩阵 左上 标 表示 目标 坐标 系 。 例 如 


ST, 表示 从 参考 坐标 系 B(ozyz) 到 目标 坐标 系 G(OXYZ ) 的 变换 和 矩阵。 








13) 当 没 有 上 下 标 时 ， 和 矩阵 用 括号 表示 。 例 如 : 
ca 0 一 Sa 一 | 
0 1 0 0.5 
T= 
sa 0 ca 0.2 


0 0 0 1 
14) 矢量 左上 标 表 示 描 述 矢量 的 坐标 系 。 那 个 上 标 说 明 矢 量 所 属 的 坐标 ， 因 此 用 那个 从 
标 系 的 单位 矢量 表达 矢量 。 例 如 : 
Sr 表示 坐标 系 G(OXYZ) 中 的 位 置 拓 量 。 
15) 矢量 的 右 下 标 表示 矢量 的 参考 点 。 例 如 : 
Srp 表示 坐标 系 G(OXYZ) PA P 的 位 置 撩 量 。 
16) 矢量 左下 标 表 示 被 测 角 度 矢量 参考 的 坐标 系 。 例 如 : 
8vp 表示 坐标 系 Booryz) 相对 于 全 局 坐标 系 G(OXYZ) 的 点 P 速度 矢量 。 
如 果 左 下 标 与 左上 标 是 相同 的 ， 那 么 可 以 省 略 左下 标 。 例 如 : 
BVp 一 3yp 
17) 角速度 矢量 右 下 标 表 示 角 速度 矢量 所 参考 的 坐标 系 。 例 如 : 
ws 表示 刚体 坐标 系 BCozyz) 的 角速度 
18) 角速度 矢量 左下 标 表 示 被 测 角速度 矢量 所 参考 的 坐标 系 。 例 如 : 
c@s 表 示 刚 体 坐 标 系 BCozyz) 相对 于 全 局 坐标 系 G(OXYZ) 的 角速度 
19) 角速度 矢量 左上 标 表 示 角 速度 矢量 所 在 坐标 系 。 例 如 : 
wn 表示 刚体 坐标 系 B 相对 于 全 局 坐标 系 G 并 且 在 刚体 坐标 系 B, 中 所 表达 的 角速度 。 
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当 角 速度 矢量 左下 标 和 左上 标 相 同时 ， 通 常 省 略 左上 标 。 例 如 : 
cO =O», 
20) 如 果 力 矢量 左下 标 是 数字 ， 则 该 数字 表示 在 串联 机 器 人 中 的 坐标 系数 。 坐 标 系 B; 
表示 在 关节 i 十 1 处 所 建立 的 坐标 系 。 例 如 : 
五 ;表示 坐标 系 B (ozyz) 中 所 测 的 在 关 市 i 十 1 处 的 力 矢 量 
在 关节 处 有 一 个 从 连 杆 i 作用 到 连 杆 G 十 1) 的 作用 力 F,; 和 一 个 从 连 杆 (i 十 1) 到 连 
杆 i 的 反作用 力 一 F;。 在 连 杆 i 上 总 是 有 来 自 连 杆 (i 一 1) 的 作用 力 F;!， 以 及 来 自 连 杆 
(i 十 1) 的 反作用 力 一 F;。 作 用 力 称 为 驱动 力 ， 反 作用 力 称 为 从 动力 。 
21) 如 果 力 矩 矢 量 右 下 标 是 数字 ， 则 该 数字 说 明 在 串联 机 器 人 中 的 坐标 系数 。 坐 标 系 
B, 表示 在 关节 i 十 1 处 所 建立 的 坐标 系 。 例 如 : 


M; 表示 坐标 系 B: (ozyz) 中 所 测 的 在 关节 G+1) 处 的 力矩 矢量 


在 关节 i 处 有 一 个 从 连 杆 i 作用 到 连 杆 Gi 十 1) 的 作用 力矩 M; 和 一 个 从 连 杆 (i 十 1) 到 
连 杆 i 的 反作用 力矩 一 M;。 在 连 杆 i 上 总 是 有 来 自 连 杆 (i 一 1) 的 作用 力矩 M;-;， 以 及 来 
自 连 杆 (i 十 1) 的 反作用 力矩 一 M;。 作 用 力矩 称 为 驱动 力矩 ， 反 作用 力矩 称 为 从 动力 矩 。 
22) 导数 算 子 的 左上 标 表示 变量 求 导 所 在 的 坐标 系 。 例 如 : 
cd Sds Bdg 
di a d 
如 果 变 量 是 一 矢量 函数 ， 定 义 矢 量 的 坐标 系 与 时 间 导 数 的 坐标 相同 的 话 ， 那 么 我 们 可 以 
使 用 下 列 简化 的 标记 ， 










































































并 用 其 简化 方程 。 例 如 : 





l de 
| 7 
"一 二 r(t)= 


23) 如 果 小 写字 母 的 后 面 跟着 角度 ， 那 么 在 数学 方程 中 小 写字 母 c 和 s 表示 余弦 cos 和 

正弦 sin 函数 。 例 如 : 
ca=cosa, sp=sing 

24) 大 写 黑体 字母 I 表示 取决 于 和 矩阵 方程 维 数 的 单位 矩阵 ， 它 是 一 个 3X3 或 4X4 单 位 
矩阵。 了 RL 用 来 区 分 单位 矩阵 的 维 数 。 例 如 : 
1 oo 
I=L=|0 1 0 
0 0 1 

25) 星 号 克 表 示 更 前 沿 的 主题 或 者 例子 ， 对 于 本 科教 学 和 初次 阅读 可 以 不 涉及 这 些 主题 
和 例子 。 

26) FATS Cll) 表示 两 平行 的 关节 轴 。 

27) 用 正 交 符号 〈 上 上) 表示 两 正 交 的 关节 轴 。 两 正 交 轴 以 直角 交叉 。 

28) 用 垂直 符号 +) 表示 两 直角 关节 轴 。 对 于 同一 参考 系 ， 两 直角 关节 轴 呈 直角 。 
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准则 0: 机 器 人 不 可 以 伤害 人 类 ， 或 者 由 于 故障 使 人 遭受 不 幸 。 

准则 1: 机 器 人 不 可 伤害 人 ， 或 者 通过 不 动作 ， 人 允许 一 个 人 受到 伤害 ， 如 果 这 不 违背 高 
一 级 的 准则 。 

准则 2: 机 器 人 必须 严格 执行 人 类 给 定 的 指令 ， 除非 所 给 的 指令 与 高 一 级 准则 发 生 


冲突 。 
准则 3: 机 器 人 必须 保护 它 自己 的 存在 ， 只 要 这 样 的 保护 不 与 高 一 级 的 准则 发 生 冲突 。 
MARR + SCRE GE (JIsaac Asimov) 提出 了 有 关 “ 机 器 人 学 ”的 这 4 个 准则 ， 以 保护 我 
们 免 受 机 器 人 智能 化 的 影响 。 虽 然 我 们 距 真 正 应 用 阿 西 莫 夫 准则 还 有 一 段 时 间 ， 并 不 立即 需 
要 ， 然 而 这 些 准则 对 于 制作 计划 是 有 好 处 的 。 

机 器 人 学 这 个 术语 指 的 是 对 机 器 人 的 研究 与 使 用 。 该 术语 于 1941 年 在 阿 西 莫 夫 的 短篇 
科幻 小 说 《借口 》 里 第 一 次 提出 。 

美国 机 器 人 学 研究 所 给 出 了 机 器 人 的 定义 : 机 器 人 是 一 个 可 重复 编程 的 多 功能 操纵 器 ， 
为 了 执行 不 同 的 任务 ， 通 过 不 同 的 程序 驱动 可 用 于 移动 材料 、 工 具 或 者 专业 化 装置 。 

从 工程 的 角度 来 说 ， 机 器 人 是 一 个 复杂 的 通用 装置 ， 它 包含 了 机 械 机 构 、 传 感 系统 和 自 
动 控制 系统 。 机 器 人 学 的 理论 基础 包括 : 力学 、 电 气 科 学 、 自 动 控 制 、 数 学 和 计算 机 科学 。 








1.1 发 展 历 史 


KAE 1938 年 ， 人 们 第 一 次 发 明了 用 于 喷涂 的 位 置 控制 装置 ， 然 而 第 一 个 现代 工业 机 
器 人 是 于 20 世纪 60 年 代 由 J. Engelberger 所 制造 的 通用 机 械 手 ， 它 是 机 器 人 市 场 中 的 第 一 
款 机 器 人 ， 因 此 Engelberger 被 称 为 机 器 人 学 之 父 。 在 20 世纪 80 年 代 ， 由 于 自动 化 工业 的 
巨大 投资 ， 机 器 人 工业 得 到 了 快速 发 展 。 

在 科研 领域 ， 第 一 个 自动 化 设备 可 能 是 Grey Walter 机 器 (1940) 和 John HEH 
足 动物 。 第 一 个 可 编程 机 器 人 是 由 George Devol 于 1954 年 设计 开发 的 。1959 年 ， 第 一 个 商 
业 化 的 可 用 机 器 人 出 现在 市 场 上 。1960 年 以 后 ， 机 器 人 机 械 手 应 用 于 工业 ， 并 且 在 20 世纪 
80 EREM KERR. 

机 器 人 的 出 现 是 遥控 器 技术 和 机 床 数控 (CNC) 技术 这 两 大 技术 融合 的 结果 。 在 第 二 次 世界 
大 战 期 间 ， 人 们 开发 了 遥控 器 ， 用 于 加 工 放射 性 材料 、 新 技术 零件 。 因 此 ， 最 初 的 机 器 人 只 不 过 
是 具有 数字 控制 的 机 械 连 接 ， 设 计 这 样 的 机 器 人 主要 是 将 材料 从 点 A 传送 到 点 B。 

今天 ， 更 复杂 的 应 用 【〈 例 如 焊接 、 喷 涂 和 装配 ) 要 求 机 器 人 有 更 多 的 运动 能 力 和 感知 能 
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力 。 因 此 ， 一 个 机 器 人 是 一 个 多 学 科 工 程 装置 。 其 中 机 械 工 程 用 来 设计 机 械 元 件 、 机 械 臂 、 
末端 执行 器 ， 也 负责 机 器 人 的 运动 学 、 动 力学 和 控制 分 析 ; 电气 工程 主要 负责 机 器 人 的 驱动 
器 、 传 感 器 、 动 力 源 和 控制 系统 ; 系统 设计 工程 负责 机 器 人 的 感知 和 控制 方法 。 编 程 或 软件 
工程 负责 机 器 人 的 逻辑 、 智 能 、 通 信和 和 联网。 

SK, KAVA 1000 多 个 与 机 器 人 相关 的 组 织 、 协 会 和 俱乐部 ， 有 500 多 种 与 机 器 人 相 
关 的 杂志 、 期 刊 及 报纸 ;每 年 有 100 多 种 与 机 器 人 有 关 的 会 议和 竞赛 ;在 大 学 里 有 50 多 门 
与 机 器 人 有 关 的 课程 。 机 器 人 有 着 巨大 的 工业 应 用 市 场 ， 并 且 能 





























N 并 
应 用 于 不 同 的 技术 操作 。 机 器 人 的 出 现 及 发 展 提高 了 工业 劳动 生 SQ % 
产 率 ， 部 分 避免 了 人 们 去 面 对 那 些 令 人 生 厌 的 、 单 调 乏 味 的 或 者 


危险 的 工作 。 而 且 ， 机 器 人 可 以 比 人 更 出 色 地 完成 许多 操作 ， 提 
供 更 高 的 精度 和 重复 性 。 除 了 工业 ， 机 器 人 还 可 用 于 极端 环境 中 。 
在 许多 领域 ， 没 有 机 器 人 的 话 ， 高 技术 标准 几乎 是 不 可 能 达到 的 。 
机 器 人 可 以 在 高 温和 低温 环境 中 工作 ， 甚 至 不 需要 光照 、 休 息 、 
新 鲜 空 气 、 薪 水 或 者 升 职 。 机 器 人 是 未 来 的 机 器 ， 它 的 应 用 范围 
正在 不 断 地 扩大 ， 其 结构 变 得 更 加 复杂 。 图 1-1 所 示 为 一 个 高 性 
能 机 械 手 。 

可 以 说 ， 机 器 人 将 以 4A 性 能 在 4D 或 者 3D3H 环境 中 工作 。 
4A 性 能 是 自动 化 〈automation)、 扩 展 〈augmentation)、 辅 助 ”四 11 高 性 能 机 械 了 
(assistance) 和 自治 (autonomous); 4D 环境 是 指 危险 (danger- 
ous), HytA (dirty). FAKE Cdull) 和 困难 (difficult) 的 环境 。3D3H 意思 是 枯燥 (dul, 
CHE (dirty)、 人 危险 (dangerous), WI (hot) 、 繁 重 (heavy) 和 危险 (hazardous) 的 环境 
工 况 。 
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器 、 处 理 器 和 相关 软件 等 部 分 构成 的 。 


1.2.1 Æ} 


构成 机 器 人 的 刚体 被 称 为 连 杆 。 在 机 器 人 学 中 ,我 们 有 时 使 用 机 械 臂 Carm) 表示 连 
杆 。 机 器 人 手臂 或 者 连 杆 是 一 刚性 元 件 ， 它 可 以 有 与 其 他 所 有 连 杆 相关 的 相对 运动 。 从 运动 
学 的 角度 看 ， 两 个 或 者 两 个 以 上 的 连 杆 连接 到 一 体 ， 若 在 它们 之 间 没 有 相对 运动 ， 那 么 这 些 
连 杆 就 可 以 作为 一 个 连 杆 考虑 。 

例 1-1 连 杆 数目 。 

图 1-2 所 示 为 含有 7 个 连 杆 和 8 个 旋转 关节 的 平面 连 杆 机 构 。 在 连 杆 3、10 和 11 之 间 没 有 
任何 相对 运动 ， 因 此 它们 可 以 作为 一 个 连 村 考虑， 例如 连 杆 3。 杆 6、12 和 13 有 相同 的 状况 ， 
它们 也 作为 一 个 连 杆 考虑 ， 例 如 杆 6。 杆 2 和 杆 8 是 刚性 连接 的 ， 只 能 当 作 一 个 连 杆 ， 例 如 杆 
2。 杆 3 和 杆 9， 跟 杆 2 和 杆 8 具有 相同 的 关系 ， 它 们 也 可 看 作 一 个 杆 ， 例 如 杆 3 。 
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1.2.2 关节 


两 个 连 杆 在 关节 处 通过 接触 而 连接 ， 在 关节 处 它 
们 的 相对 运动 可 用 同一 坐标 表示 。 典 型 的 关节 要 么 是 
旋转 的 ， 要 么 是 棱柱 的 。 图 1-3 描述 了 旋转 关节 和 楼 
柱 关 节 的 几何 形式 。 旋 转 关 节 RMA TBE. RY 
在 两 连 杆 之 间 有 相对 的 旋转 。 棱 柱 关 节 P 允许 在 两 连 
杆 之 间 有 相对 移动 。 

如 果 旋 转 关 节 将 两 连 杆 连接 起 来 ， 它 们 将 绕 着 图 1-2 含有 7 个 连 杆 和 8 个 旋转 
一 条 线 发 生 相 对 转动 ， 这 条 线 称 为 关节 轴线 。 如 果 关节 的 平面 连 杆 机 构 
棱柱 关节 将 两 连 杆 连接 起 来 ,它们 将 沿 着 一 条 直线 
发 生 移 动 ， 这 条 线 也 称 为 关节 轴线 。 在 关节 处 ， 描 述 两 被 连接 连 杆 的 单一 坐标 值 称 为 
关节 坐标 或 者 关节 变量 。 对 于 旋转 关节 ， 关 节 变 量 是 一 角度 ; 对 于 棱柱 关节 ， 关 节 变 
量 则 是 距离 。 

在 机 器 人 中 旋转 关节 和 棱柱 关节 的 符号 说 明 分 别 如 图 1-4a 一 图 1-4c 和 图 1-5a 一 图 1-5c 所 示 。 


srt 


旋转 关节 楼 柱 关节 b) 9) 






































图 1-3 ”旋转 关节 和 楼 柱 关节 说 明 图 1-4 机 器 人 旋转 关节 D 的 符号 说 明 








主动 关节 的 坐标 由 驱动 器 控制 ， 而 从 
动 关节 则 没有 驱动 需 。 从 动 关节 变量 是 主 
动 关节 变量 和 机 械 臂 几何 参数 的 函数 。 因 > 
此 ， 从 动 关节 也 可 称 为 非 主动 关节 或 者 日 
由 关节 。 as g 
主动 关节 通常 是 移动 或 者 转动 ， 然 而 
从 动 关节 可 以 是 任何 能 够 提供 面 接触 的 低  ， b 9 
副 关节 。 有 6 种 不 同 的 低 副 关节 ， 分 别 图 1-5 机 器 人 棱柱 关节 〈 副 ) 的 符号 说 明 
为 : 旋转 副 、 移 动 副 、 圆 柱 副 、 螺 旋 副 、 
球 副 以 及 平面 副 。 旋 转 副 关 节 和 移动 副 关节 是 最 常用 的 关节 ， 它 们 用 在 串联 机 械 手 中 。 其 他 
的 关节 类 型 只 不 过 是 为 了 完成 相同 的 功能 或 者 提供 附加 自由 度 的 一 个 实现 。 棱 柱 关 节 (移动 
副 ) 和 旋转 关节 (旋转 副 ) 提供 一 个 自由 度 ， 因 此 一 个 机 械 手 的 关节 数 就 是 该 机 械 手 的 自由 
度 。 典 型 的 机 械 手 应 该 至 少 有 6 个 自由 度 ，3 个 自由 度 用 于 定位 ，3 个 自由 度 用 于 定向 。 具 
有 6 个 以 上 自由 度 的 机 械 手 在 运动 学 上 就 是 元 余 机 械 手 。 
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1.2.3 机 械 手 


由 连 杆 、 关 节 和 其 他 结构 零 部 件 所 构成 的 机 器 人 主体 称 为 机 械 手 。 当 一 个 机 械 手 上 装 了 
手腕 、 夹 持 器 和 控制 系统 时 ， 该 机 械 手 就 变 成 了 一 个 机 器 人 人。 然而， 在 文学 作品 中 的 机 器 人 
与 机 械 手 是 等 效 的 ， 均 指 机 器 人 。 图 1-6 所 示 为 
3R 机 械 手 原理 图 。 


1.2.4 m+ Be 


在 前 臂 与 未 端 执行 器 之 间 的 机 器 人 运动 链 中 。_ 
的 关节 指 的 就 是 手腕 。 用 球 关节 设计 机 械 手 是 普 ° 
遍 的 做 法 ， 这 意味 着 在 球 关节 处 3 个 旋转 关节 轴 
线 在 同一 关节 处 相互 交叉 ， 这 个 共同 的 球 关节 称 
为 腕 关节 。 图 1-7 所 示 为 3R 机 械 机 构 的 球形 手腕 
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运动 学 原理 。 
球形 腕 关节 极 大 地 简化 了 运动 学 分 析 ， 它 允 基 座 
许 我 们 解 耦 末端 执行 器 的 定位 和 定向 。 因 此 ， 机 图 1-6 3R 机 械 手 原理 图 








MPA 3 个 定位 自由 度 ， 它 们 是 由 机 械 臂 上 的 3 个 
关节 所 提供 的 。 这 时 定向 的 自由 度数 则 取决 于 手腕 。 依 据 应 用 ， 我们 可 以 设计 一 手腕 ,使 之 
有 1 个、 两 个 或 者 3 个 自由 度 。 























连接 至 前 辟 
图 1-7 球形 手腕 的 运动 学 原理 


1.2.5 末端 执行 器 


末端 执行 器 是 安装 在 最 后 一 根 连 杆 上 的 元 件 ， 用 于 完成 机 器 人 所 要 求 的 工作 。 最 简单 的 
末端 执行 器 就 是 夹 持 器 ， 它 通常 只 有 两 个 动作 ， 张 开 和 闭合 。 机 器 人 中 机 械 辟 和 手腕 的 装配 
基本 上 用 于 定位 末端 执行 器 或 者 能 够 执行 的 任何 机 具 ， 就 是 末端 执行 器 或 者 机 具 实 际 执行 任 
务 。 目 前 大 量 的 研究 主要 致力 于 特殊 用 途 末 端 执行 器 和 机 具 的 设计 。 也 有 大 量 的 关于 开发 仿 
人 和 手 的 研究 。 相 关 人 员 现 已 经 开发 出 了 这 样 的 仿 人 手 ， 主 要 用 于 制造 中 的 修复 工作 。 因 此 ， 
机 器 人 是 由 一 个 机 械 手 或 者 主机 、 一 个 手腕 及 一 个 机 具 组 成 。 手 腕 和 末端 执行 器 装配 在 一 
起 ， 也 称 为 机 械 手 。 
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1.2.6 驱动 器 


驱动 器 作为 机 器 人 的 “肌肉 ”， 能 改变 机 器 人 的 配置 。 驱 动 器 提供 作用 于 机 械 机 构 上 的 
动力 ， 抵 制 重力 、 惯 性 和 其 他 外 力 ， 以 便 修正 机 器 人 的 几何 位 置 。 驱 动 器 可 以 是 电气 、 液 压 
或 者 气动 类 型 ， 并 且 必 须 是 可 控 的 。 

1.2.7 传感器 

用 于 检测 和 收集 关于 内 外 环境 状况 信息 的 元 件 就 是 传感器 。 依 据 本 书 的 范围 ， 关 节 位 
置 、 速 度 、 加 速度 就 是 最 重要 的 传 感 信息 。 集 成 在 机 器 人 中 的 传感器 把 关于 每 根 连 杆 和 关节 
的 信息 发 送 到 控制 单元 ， 控 制 单元 再 决定 机 器 人 的 配置 。 

1.2.8 控制 器 


控制 器 或 者 控制 单元 有 3 个 作用 : 

1) 信息 作用 ， 收 集 和 处 理由 机 器 人 传感器 所 提供 的 信息 。 

2) 决策 作用 ， 规 划 机 器 人 机 构 的 几何 运动 。 

3) 通信 作用 ， 组 织 机 器 人 及 其 环境 之 间 的 信息 。 控 制 单元 包括 处 理 咒 和 软件 。 
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美国 机 器 人 学 协会 将 机 器 人 分 成 下 面 的 第 3 至 第 6 类 。 法 国 机 器 人 技术 协会 将 第 2、3、 
4 类 合成 同一 类 型 ， 将 机 器 人 分 成 4 类 。 日 本 工业 机 器 人 协会 将 机 器 人 分 成 6 种 不 同类 型 。 

第 1 类， 人工 操 作 机 (Manual handling devices): 它 是 一 种 由 操作 者 控制 的 多 自由 度 

第 2 类， 固定 顺序 机 器 人 (Fixed sequence robot): 能 依据 既定 程序 ， 执 行 某 一 任务 的 
相应 阶段 。 

第 3 类 ， 可 变 顺 序 机 器 人 (Variable sequence robot): 能 依据 既定 但 是 可 编程 的 程序 ， 
执行 某 一 任务 的 相应 阶段 。 

第 4 类 ， 示 教 再 现 机 器 人 (Playback robot): 操作 者 通过 人 工 引 导 机 器 人 执行 任务 ， 这 
个 执行 任务 的 运动 过 程 能 被 记录 下 来 ， 并 用 于 后 面 的 示 教 再 现 。 依 据 所 记录 的 信息 机 器 人 能 
重复 相同 的 运动 。 

第 5 类， 数字 控制 机 器 人 (Numerical control robot): 操作 者 给 机 器 人 提供 运动 程序 ， 
而 不 是 人 工 教授 机 器 人 完成 任务 。 

第 6 类， 智能 机 器 人 (Intelligent robot): 机 器 人 能 够 理解 其 周围 环境 ， 即 使 所 执行 任 
务 的 周围 环境 发 生变 化 ， 也 能 够 成 功 完 成 任务 ， 

除了 这 些 官方 的 分 类 外 ， 还 可 通过 其 他 标准 对 机 器 人 进行 分 类 ， 例 如 几何 结构 、 工 作 空 
间 、 了 驱动 、 控 制 及 应 用 。 


1.3.1 几何 结构 
如 果 机 器 人 的 运动 结构 不 构成 环 链 ， 那 么 该 机 器 人 被 称 为 串联 或 者 开 环 机 械 手 。 如 果 它 
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的 结构 形成 了 闭环 链 ， 那 么 该 机 器 人 被 称 为 平行 机 械 手 或 者 闭环 机 械 手 。 如 果 机 器 人 的 结构 
是 由 开 环 链 和 闭环 链 共 同 构成 ,那么 它 就 是 一 个 混合 机 械 手 。 

作为 一 机 械 系统 ， 我 们 可 把 机 器 人 当 作 是 在 一 些 关 节 处 连接 在 一 起 的 一 组 刚体 。 关 节 可 
能 是 旋转 的 ， 也 可 能 是 平移 的 ， 因 为 任何 其 他 类 型 的 关节 都 可 通过 这 两 种 简单 关节 的 合成 进 
行 构建 。 

大 部 分 工业 机 械 手 有 6 个 自由 度 。 从 地 面 关节 开始 的 前 3 个 关节 ， 可 以 划分 成 开 环 机 械 
手 。 用 这 两 种 类 型 的 关节 在 数学 上 有 72 种 不 同 的 工业 机 械 手 配置 ， 因 为 每 一 种 关节 可 能 是 
P 〈 平 动 关节 ) RR ARN). MARKI AAT REAR TAY Cll). EAA Ch) 或 
SRE (+). MEAR AIA AEM. PAT PE CH, AT ERE 
呈现 空间 直角 。 两 垂直 的 关节 轴 ， 如 果 其 中 一 个 轴 绕 着 公共 法 线 旋转 90°, ABA ix PA HE EY 
关节 轴 就 变 成 了 平行 轴 。 如 果 它 们 的 公共 法 线 趋 于 零 ， 那 么 两 垂直 关节 轴 就 变 成 了 同 面 
直角 





















































































































































除了 72 种 可 能 的 机 械 手 配置 外 ， 最 重要 的 机 械 手 配置 包括 : R || R || P (SCARA), R 
LRR (articulated, 回转 的 )、R ERP (spherical, 球形 的 )、R 上 P HP (cylindrical, P 
柱 的 ) 以 及 P FP EP (Cartesian， 空 间 直 角 坐 标的 )，。 

1. RIRI P 结构 

R|| R|| P (SCARA) 机 械 手 〈 用 于 装配 的 关节 式 旋转 机 器 人 ) 如 图 1-8 所 示 ， 它 是 常 
用 机 械 手 ， 它 的 名 字 表 明 该 机 械 手 多 用 于 装配 操作 。 























图 1-8 RIRIP 机 械 手 


2. R FRAR 结构 

如 图 1-6 所 示 的 R FRR 配置， 被 称 为 肘 形 机 械 手 、 旋 转机 械 手 、 回 转机 械 手 或 者 拟人 
机 械 手 。 它 对 于 工业 机 器 人 是 特别 适合 的 配置 ，25% 的 PUMA 工业 机 器 人 都 是 这 种 类 型 的 配 
置 。 因 为 它 的 重要 性 ， 旋 转机 器 人 的 结构 如 图 1-9 所 示 ， 相 关 术 语 用 于 说 明 不 同 元 件 的 名 称 。 

3. R 上 FR 一 P 结构 

对 于 小 型 机 器 人 ， 球 形 结构 是 一 个 合适 的 结构 。 几 乎 15% 的 工业 机 器 人 ， 例 如 
(Stanford) 机 械 手 ， 都 是 由 这 种 结构 制造 的 。R EROP 球形 结构 如 图 1-10 所 示 。 

通过 用 平 动 关节 取代 关节 机 械 手 中 的 第 3 个 关节 ， 我 们 可 得 到 球形 结构 机 械 手 。 球 形 机 
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图 1-9 RT 上 RJ 十 R 肘 (关节) 机 器 人 的 结构 





末端 执行 铝 位 置 。 


械 手术 语 来 自 球面 坐标 相对 于 基 座 坐标 系 所 定义 的 


1-11 原理 性 地 说 明了 斯 坦 福 机 械 手 ， 它 是 





最 著名 的 球形 机 融 人 之 一 。 
4. R||P FP 结构 


对 于 中 型 载荷 机 器 人 ， 圆 相 








FE 形 结构 是 一 个 


E% 


合适 的 结构 ， 差 不 多 45% 的 工业 机 器 人 都 采用 这 种 
结构 类 型 。R | PEP 结构 如 图 1-12 所 示 。 圆 柱 形 
机 械 手 的 第 1 个 关节 是 旋转 的 ， 绕 基 座 产生 旋转 运 
动 ; 同时 第 2 个 关节 和 第 3 个 关节 是 移动 的 。 如 名 























字 所 表明 的 那样 ， 关 节 变 量 相对 于 基 座 是 末端 执行 


A HY [Bal EF A op EB E o 
5. P HP HP 结构 











图 1-10 BLAH R ERP 球形 结构 


Xt PRK aR A. TAB LAG tn JB R at — BPE AT. PEAR 15% TMV BL 
器 人 都 是 由 这 种 结构 制造 的 。P FP FP 结构 如 图 1-13 所 示 。 
对 稍 卡 儿 坐 标 形 机 械 手 ， 关 节 变 量 相对 于 基 座 是 末端 执行 吉 的 笛 卡 儿 坐 标 变量 。 就 像 所 




















1.3.2 工作 空间 














期 望 的 那样 ， 机 械 手 的 运动 描述 是 所 有 机 械 手 中 最 简单 的 。 笛 卡 儿 华 标 形 机 械 手 对 于 台式 装 
AC ASS Wie COATES Lat AD 是 有 用 的 。 


机 械 手 工作 空间 是 指 末 端 执行 器 所 能 到 达 的 空间 体积 。 工 作 空间 受 限 于 机 械 手 结构 和 机 





械 的 关节 约束 。 工 作 空 间 被 分 为 可 到 达 的 工作 空间 和 灵活 的 工作 空间 。 可 到 达 的 工作 空间 就 


是 在 至 少 一 个 方向 由 末端 执行 器 所 引起 的 每 个 关节 所 到 达 的 空间 体积 ;灵活 的 工作 空间 是 指 
在 所 有 可 能 的 方向 上 由 末端 执行 器 所 引起 的 每 个 关节 所 到 达 的 空间 体积 。 灵 活 的 工作 空间 是 











可 能 到 达 工 作 空 间 的 一 个 子 集 。 
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图 1-11 斯 坦 福 机 械 手 : 一 个 人 R FR 二 P 球 形 机 器 人 























图 1-12 R| P FP 机械 手 圆 柱 形 结构 图 1-13 P HP FP LRF A-F JLA RJE Et 








大 部 分 开 环 机 械 手 用 手腕 部 件 设 计 ， 手 部 部 件 连接 至 主要 的 3 个 连 杆 组 件 。 因 此 ， 前 3 
个 连 杆 是 比较 长 的 ， 主 要 用 来 定位 ， 而 手腕 用 来 控制 和 定位 末端 执行 器 。 这 就 是 为 什么 由 前 
3 个 组 件 所 构成 的 组 件 被 称 为 手臂 的 原因 ， 由 其 他 连 杆 所 构成 的 组 件 称 为 手腕 。 


1.3.3 驱动 


驱动 器 将 动力 转换 成 运动 。 机 器 人 典型 地 依靠 电气 、 液 压 或 者 气动 来 驱动 。 也 可 能 考虑 
其 他 的 驱动 类 型 ， 如 压 电 驱动 、 磁 致 伸缩 驱动 、 形 状 记 忆 合 金 驱 动 和 聚合 物 驱 动 。 

电气 驱动 的 机 器 人 ， 其 动力 源 是 交流 电动 机 或 直流 电动 机 。 电 气 驱 动机 器 人 也 是 最 易 被 
接受 的 机 器 人 。 相 较 于 液压 驱动 和 气压 驱动 的 机 器 人 ,电气 驱 动机 器 人 干净 、 安 静 、 精 密 ， 
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动机 高 速 回 转 ， 因 此 高 减速 比 齿轮 箱 用 来 降低 电动 机 转速 。 在 电力 损失 情况 下 ， 滞 后 操作 
灵活 性 和 上 自制 动 是 高 减速 齿轮 箱 的 一 个 优势 。 然 而 ， 当 要 求 有 高 速 或 者 高 载 能 力 时 ， 电 动机 
驱动 是 不 能 跟 液压 驱动 相 比 的 。 

在 高 速 、 高 转 矩 质量 比 或 者 高 功率 质量 比 的 工 况 下 ， 液 压 驱 动 是 令 人 满意 的 。 因 此 ， 液 
压 驱 动机 器 人 主要 用 于 举 抬 重 载 。 除 了 噪声 大 和 趋 于 泄漏 的 缺点 外 ， 液 压 驱 动 的 缺点 还 包括 
它 需 要 有 一 个 必需 的 液压 和 于 和 其 他 硬件 。 

气压 驱动 机 器 人 是 比较 便宜 和 简单 的 ， 但 是 它 不 能 被 精密 控制 。 除 了 低 精 度 运动 外 ， 它 
也 像 液压 驱动 机 器 人 一 样 有 着 与 其 大 部 分 相同 的 优点 和 缺点 。 


1.3.4 控制 


通过 控制 方法 ， 机 器 人 被 分 为 伺服 〈 闭 环 控 制 ) Bla AAA fe kk 〈 开 环 控制 ) 机 器 人 。 
伺服 机 器 人 使 用 闭环 计算 机 控制 决定 其 运动 ， 它 是 一 个 真实 多 功能 的 可 重复 编程 装置 。 依 据 
控制 絮 对 末端 执行 器 的 引导 ,伺服 控制 机 器 可 被 进一步 分 类 。 

































































但 是 当 末 端 执 行 器 路 径 处 在 点 位 之 间 时 ， 点 位 机 器 人 是 不 可 控制 的 。 另 外 一 方面 ， 在 连续 路 
径 机 器 人 中 ,末端 执行 器 在 整个 路 径 是 能 够 被 控制 的 。 例 如 ， 可 以 教授 机 器 人 末端 执行 器 沿 
着 两 点 之 间 的 直线 甚至 是 沿 某 一 轮廓 ， 如 焊 颖 移动 。 除 此 之 外 ， 末端 执行 器 的 速度 和 /或 者 
加 速度 通常 也 被 控制 。 大 部 分 先进 的 机 器 人 要 求 有 最 精密 的 计算 机 控制 器 和 软件 开发 。 

非 伺 服 机 器 人 本 质 上 是 开 环 装置 ， 它 的 运动 限于 预定 的 机 械 停靠 站 ， 主 要 用 于 材料 搬运 。 


1.3.5 应 用 


不 论 机 器 人 的 大 小 ,依据 其 应 用 范围 主要 可 分 为 装配 机 器 人 和 非 装 配 机 器 人 人。 然而， 在 
工业 中 ， 应 用 机 右 人 也 可 被 分 为 如 下 种 类 ， 如 机 器 加 载 机 器 人 、 拾 捡 与 放置 机 器 人 、 焊 接 机 
器 人 、 绘 图 机 器 人 、 装 配 机 器 人 、 检 查 机 器 人 、 采 样机 器 人 、 制 造 生产 机 器 人 、 生 物 医 疗 机 
器 人 、 辅 助 机 器 人 、 远 程控 制 移 动机 器 人 人、 网络 机 句 人 。 

依据 设计 特性 ， 大 部 分 工业 机 器 人 手臂 是 拟人 的 ， 在 一 定 程度 上 它们 有 一 个 “肩膀 ” 
(前 两 个 关节 )、 一 个 “ 肘 部 ”( 第 3 个 关节 ) 和 一 个 “手腕 ”( 最 后 3 个 关节 )。 因 此 ， 总 体 
上 工业 机 器 人 手臂 想 要 把 一 个 物体 放置 在 任何 位 置 ， 需 有 6 个 自由 度 。 

大 部 分 商业 串联 机 械 手 仅仅 有 旋转 关节 。 相 较 于 移动 关节 ， 旋 转 关 节 成 本 低 ， 对 同一 机 
器 人 来 说 旋转 关节 能 提供 一 个 较 大 的 灵活 工作 空间 。 在 精度 无 损 的 情况 下 ， 相 较 于 串联 机 器 
人 能 够 搬 动 的 最 大 载荷 ， 它 是 非常 笨重 的 。 它 们 的 有 效 载 荷 与 自身 重量 之 比 小 于 1/10。 为 
了 刚性 工作 ， 这 种 机 器 人 连 杆 必须 坚硬 ， 因 此 它 是 如 此 的 策 重 。 对 于 工业 机 械 手 而 言 ， 正 向 
《前 向 ) 和 道 向 (后 向 〉 定 位 的 简化 ， 以 及 速度 运动 学 的 简化 是 主要 的 设计 准则 之 一 。 因 此 ， 
几乎 所 有 的 串联 机 器 人 都 有 一 个 特殊 的 运动 结构 。 












































1.4 机 器 人 运动 学 、 动 力学 、 控 制 概述 





正 向 运动 学 问题 就 是 求解 关节 坐标 的 运动 学 参数 ， 这 些 运 动 学 参数 再 用 来 求解 稍 卡 儿 坐 
标 系 下 的 相关 参数 。 逆 向 运动 学 问题 就 是 求解 笛 卡 儿 空 间 中 末端 执行 器 的 运动 学 参数 ， 这 些 
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运动 参数 在 关节 空间 中 是 必需 的 。 道 向 运动 学 是 高 度 非 线性 的 ， 它 与 正 向 运动 学 问题 相 比 通 
稼 是 非常 困难 的 问题 。 只 要 逆向 定位 问题 已 经 被 解决 ， 逆 向 速度 和 加 速度 问题 是 线性 的 ， 并 
且 比 较 简 单 。 

运动 学 (Kinematics) 是 来 自 希腊 语 kinua (movement) 的 法 语词 语 cinematique 的 英 
语 版 。 运 动 学 是 分 析 运 动 科学 的 一 个 分 支 ， 该 学 科 不 注重 什么 原因 引起 的 运动 。 通 过 运动 我 
们 表示 任何 类 型 位 移 ， 包 括 空 置 和 方向 的 改变 。 因 此 ， 位移 、 相 对 于 时 间 的 连续 导数 ， 速 
度 、 加 速度 、 加 加 速度 ， 所 有 的 这 些 都 归 和 运动 学 。 

定位 就 是 使 末端 执行 器 在 工作 空间 范围 内 到 达 一 个 任意 点 ， 而 定向 就 是 使 末端 执行 器 移 
向 在 某 位 置 处 所 要 求 的 方向 。 定 位 是 机 械 辟 的 工作 ， 定 向 是 手腕 的 工作 。 为 了 简化 运动 学 分 
析 ， 我 们 可 以 解 看 末端 执行 恬 的 定位 和 定 问 。 

基于 运动 形式 ,一 个 6 自由 度 机 器 人 包含 6 个 序列 的 可 移动 连 杆 和 6 个 关节 ， 其 中 至 少 
最 后 两 个 连 杆 的 长 度 为 零 。 

一 般 说 来 ， 几 乎 所 有 的 运动 学 问题 都 可 以 用 矢量 加 法 来 解释 。 然 而 ， 在 矢量 方程 中 ， 每 
个 矢量 必须 在 同一 参考 坐标 系 中 转换 和 表达 。 

动力 学 是 用 来 研究 系统 随 着 时 间 变 化 所 经 历 的 状态 变化 的 。 在 机 械 系统 中 ， 如 机 器 人 ， 
状态 变化 涉及 运动 。 在 系统 动力 学 分 析 中 ， 求 系统 运动 方程 的 导数 是 主要 的 步 又， 因为 在 系 
统 设计 、 分 析 和 控制 中 运动 方程 是 必 不 可 少 的 。 运 动 的 动力 学 方程 描述 动力 学 行为 。 动 力学 
方程 主要 用 于 机 峰 人 运动 的 计算 机 仿真 ， 设 计 适 当 的 控制 方程 ， 以 及 对 所 设计 的 动力 学 系统 
性 能 进行 评估 。 
与 运动 学 类 似 ， 机 器 人 动力 学 问题 可 以 作为 正 向 和 逆向 问题 考虑 。 在 正 向 动力 学 方程 
对 于 在 关节 处 给 定 的 初始 条 件 和 转 和 矩 ， 我 们 应 该 预测 机 器 人 的 运动 。 在 逆向 动力 学 问题 
我 们 应 该 计算 对 于 给 定 的 位 置 、 速 度 和 加 速度 形成 预定 的 轨迹 所 必需 的 力 和 转 和 。 
机 器 人 控制 问题 以 末端 执行 器 期 望 运动 为 主要 特征 。 当 控制 系统 要 求 在 关节 坐标 中 输入 
机 器 人 期 望 的 运动 轨迹 可 以 从 其 在 笛 卡 儿 坐 标 系 轨 迹 中 指定 。 
传感器 用 来 采集 数据 ， 以 便 找 到 在 关节 空间 中 机 器 人 的 真实 状态 。 这 就 意味 着 一 个 要 
求 ， 即 需要 将 在 笛 卡 儿 坐 标 系 中 所 表达 的 运动 变量 转换 到 其 等 效 的 关节 空间 中 去 。 这 些 转 换 
绝对 独立 于 机 械 手 的 运动 结构 。 因 此 ， 机 融 人 控制 包括 以 下 3 个 计算 问题 : 

1) 箭 卡 儿 坐 标 系 中 轨迹 的 确定 。 

2) 从 笛 卡 儿 轨 迹 到 等 效 关节 坐标 空间 的 转换 。 

3) 形成 实现 轨迹 的 电动 机 转 矩 指令 。 


*1.41 三 元 组 


考虑 任何 4 个 非 共 面 的 点 O、A、B 和 C。 三 元 组 OABC 由 3 KHZ OA, OB 和 OC 
组 成 ， 形 成 一 个 刚体 。 如 果 直 线 OA 上 的 点 A 在 O 点 的 同 侧 ， 那 么 点 A 是 无 关 紧 要 的 ， 点 
B 和 点 C 也 是 类 似 的 。 在 平面 OAB 内 ， 绕 着 O 点 旋转 OB， 以 使 得 AOB 为 90"，OB 的 
旋转 角 小 于 90"。 接 下 来 ,旋转 直线 OC 直至 其 垂直 于 平面 OAB，OC 的 旋转 角 小 于 90”。 我 
们 说 通过 连续 变形 驱使 正 交 的 三 元 组 ， 得 到 一 个 新 的 位 置 。 任 何 正 交 三 元 组 在 三 元 组 
OABC LEK., 

假设 一 个 正 交 三 元 组 OABC， 将 点 A 移动 到 O 点 的 另外 一 侧 即 可 得 到 另外 一 个 相反 的 
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三 元 组 OA'BC， 
MARE: THAZLEATAEZJIH OABC, HAEA FHH HEH OA’ 
这 两 个 三 元 组 OABC 和 OA'BC 中 的 任何 一 个 可 定义 为 正三 元 组 ， 作 为 标准 使 用 ， 这 
个 定义 为 负 三 元 组 。 然 而 我 们 通常 选择 右手 定 则 为 正 ， 从 OA 到 OB 的 旋转 方向 沿 着 
OC 方向 形成 右手 螺旋 。 右 手 〈 正 的 ) 正 交 三 元 组 并 不 重 铸 于 左手 ( 负 的 ) 三 元 组 。 这 样 一 
来 ， 就 有 两 个 本 质 明 显 区 别 的 三 元 组 类 型 。 这 就 是 三 维 空间 的 本 质 特征 。 


1.4.2 单位 矢量 


由 单位 矢量 i、 7 、 构成 的 一 个 正 交 三 元 组 是 3 个 单位 矢量 的 一 个 集合 ， 它 的 方向 形 
成 了 一 个 正 交 三 元 组 。 根 据 这 个 定义 ， 我 们 有 : 





















































?=1, j?=1, R2 一 1 (1-1) 
而 且 ， 由 于 7 XR 平行 于 单位 矢量 ; ， 根 据 矢 量 又 乘积 的 定义 我 们 有 : 
XEk=i (1-2) 
类 似 地 ， 有 
kXi=j (1-3) 
i XGSE (1-4) 
í e j=0 (1-5) 
了 .有 一 0 (1-6) 
k+i=0 (1-7) 
(Xk) ei=iei=l (1-8) 
任何 矢量 r 都 可 以 置 于 下 列 形式 的 正 交 条 件 中 : 
r=(rijit(rsj)\i tr R)R (1-9) 
矢量 加 法 是 运动 学 中 关键 的 操作 。 然 而 ， 只 有 当 矢 量 在 同一 坐标 系 中 表示 时 矢量 才能 相 
加 ， 这 点 要 特别 注意 。 这 样 ， 不 表示 在 同一 坐标 系 中 的 矢量 ， 矢 量 方程 就 失去 了 意义 ， 
例如 : 
a =b +c (1-10) 
正确 的 表达 为 
Ba ="b +? ¢ (1-11) 


1.4.3 参考 坐标 系 和 坐标 系统 


在 机 器 人 学 中 ， 我 们 可 在 机 器 人 的 每 根 连 杆 上 和 机 器 人 环境 中 的 每 个 物体 上 建立 一 个 或 
者 更 多 的 坐标 系 。 如 此 一 来 ， 坐 标 系 间 的 变换 ， 称 为 坐标 变换 ， 这 是 在 机 器 人 建 模 与 编程 中 
的 一 个 基本 概念 。 

刚体 的 角 运 动 可 以 用 好 几 种 方式 描述 ， 最 常用 的 方式 如 下 : 

D 关于 右手 定 则 的 全 局 固定 直角 坐标 轴 的 一 组 旋转 运动 。 

D 关于 右手 定 则 的 运动 直角 坐标 轴 的 一 组 旋转 运动 。 

) 空间 中 国定 轴 的 角 位 移 。 

参考 坐标 系 是 分 析 人 员 用 来 描述 连 杆 运动 的 一 个 特殊 视角 。 固 定 坐 标 〔( 固 定 基 座 ) 是 一 
个 无 运动 且 固 定 于 地 面 的 参考 坐标 系 。 机 器 人 运动 发 生 在 被 称 为 全 局 坐标 系 的 固定 坐标 系 
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中 。 运 动 坐 标 系 是 一 个 与 连 杆 相连 的 参考 坐标 系 。 每 根 移 动 的 连 杆 都 有 一 个 附带 的 参考 坐标 
系 ， 该 参考 坐标 系 与 连 杆 相连 并 且 接 受 连 杆 的 每 项 运动 。 运 动 参考 坐标 系 又 称 为 局 部 参考 坐 
标 系 。 连 杆 相 对 于 地 面 的 位 置 和 方向 ， 可 以 用 位 于 全 局 坐标 系 中 的 局 部 坐标 系 位 置 和 方向 来 
解释 。 在 机 器 人 分 析 中 ， 我 们 将 全 局 坐标 系 固定 于 地 面 ， 并 且 将 局 部 参考 坐标 系 与 每 根 连 杆 
相连 。 

坐标 系统 与 参考 坐标 系 稍微 有 所 不 同 。 坐 标 系 统 决定 在 每 一 参考 坐标 系 中 描述 运动 的 方 
式 。 币 卡 儿 坐 标 系 是 机 器 人 学 中 最 常用 的 坐标 系 ， 但 是 圆柱 的 、 球 形 的 和 其 他 的 坐标 系 也 可 
以 使 用 。 
此 后 ,我 们 将 等 效 使 用 参考 坐标 系 、 运 动 坐标 系 和 坐标 系统 ， 因 为 第 卡 儿 坐标 系 是 我 们 
使 用 的 唯一 坐标 系 。 

刚体 B 上 一 点 P 的 位 置 可 用 矢量 x 描述 。 如 图 1-14 所 示 ， 点 P 的 位 置 和 撩 量 可 以 在 全 局 
坐标 系 中 分 解 为 









































Gr =XI+YJ+ZK (1-12) 
或 者 在 刚体 坐标 系 中 分 解 为 
Br =zityjtzk (1-13) 





= 
图 1-14 点 己 的 位 置 矢 量 在 刚体 坐标 系 B 或 全 局 坐标 系 G 中 的 分 解 











RAX, Y, ZUR, ys z 分 别 被 称 为 点 P 在 全 局 坐标 系 和 局 部 坐标 系 的 坐标 分 
。 对 于 数学 计算 来 说 ， 利 用 列 矩 阵 表述 矢量 Sr 和 Br 是 有 效 的 ， 列 矩阵 由 下 面 分 量 构成 : 


rl 











x 

Gp = | (1-14) 
Z 
a 

Br = (1-15) 


机 器 人 运动 学 主要 研究 位 置 、 速 度 和 加 速度 ， 而 不 考虑 引起 这 些 运动 的 作用 力 。 矢 量 和 
参考 坐标 系 是 分 析 复 杂 系 统 运动 的 主要 工具 ， 特 别 是 当 运动 是 三 维 空间 的 以 及 涉及 许多 零 部 
件 时 。 

坐标 系 是 由 一 组 基本 的 矢量 所 定义 的 ， 例 如 沿 着 3 个 坐标 轴 的 单位 矢量 。 因 此 旋转 矩阵 
作为 坐标 变换 矩阵 ， 也 可 以 用 来 定义 从 一 个 坐标 系 到 另 一 个 坐标 系 的 基体 变化 。 

旋转 矩阵 可 用 以 下 3 种 方式 来 解释 : 
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1) 映射 。 它 主要 表述 坐标 变换 ， 上 映射 和 点 PP 在 两 个 不 同 坐标 系 中 的 相关 坐标 。 

2) 坐标 描述 。 相 对 于 固定 坐标 系 ， 它 给 出 变换 坐标 系 的 定向 。 

3) 算法 。 将 一 个 矢量 旋转 ， 形 成 一 个 新 的 矢量 。 

刚体 旋转 可 用 旋转 矩阵 尺 、 欧 拉 角 (Euler angles)、 角 轴 公 约 (angle-axis convention) 
以 及 四 元 数 (quaternion)〉 来 描述 ， 每 种 方法 都 有 其 优 缺 点 。 

在 基体 变化 中 ， 旋 转 和 矩阵 R 是 正 向 解释 ， 与 此 同时 其 缺点 是 必须 知道 9 个 合成 的 参数 。 
每 个 单独 参数 失去 了 其 物理 意义 ， 只 有 在 整体 矩阵 上 才 有 意义 。 

欧 拉 角 是 通过 由 绕 着 局 部 (有 了 时 全 局 ) 坐标 系 的 3 个 坐标 轴 进 行 各 自 连 续 旋 转 来 粗略 定 
义 的 。 使 用 欧 拉 角 的 优点 是 旋转 和 运动 可 由 3 个 具有 普通 物理 意义 的 独立 参数 描述 。 其 缺点 是 
描述 不 独立 并 会 产生 奇异 性 问题 。 计 算 复 合 旋转 也 并 不 是 一 件 简 单 的 事情 ， 除 扩展 成 矩 
阵 外 。 

角 轴 是 能 最 直观 地 描述 旋转 运动 的 。 但 是 ， 它 要 求 4 个 参数 能 合成 为 一 个 单一 的 旋转 运 
动 ， 合 成 旋转 运动 的 计算 并 不 简单 ， 对 于 小 旋转 来 说 它 是 病态 的 。 

四 元 数 在 直观 保存 角 轴 方面 是 很 好 的 ， 并 能 克服 小 旋转 的 病态 ， 确 认 一 组 结构 允许 合成 
旋转 运动 的 计算 。 四 元 数 的 缺点 是 4 个 参数 必须 能 够 表述 一 个 旋转 运动 。 参 数 化 比 角 轴 更 复 
杂 ， 并 且 有 时 失去 了 其 物理 意义 。 四 元 数 相 乘 并 不 像 徐 阵 相 乘 那样 简单 明了 。 






























































1.44 矢量 函数 


矢量 是 研究 运动 学 和 动力 学 的 基础 。 人 位置、 速度、 加速度、 动量、 力 和 力矩， 这 些 都 是 
矢量 。 依 据 一 个 已 知 的 参考 ， 矢 量 位 于 一 点 上 。 矢 量 由 幅 值 、 方 向 和 参考 原点 构成 。 我 们 必 
须 明确 地 标明 矢量 的 这 些 要 素 。 

如 果 一 个 矢量 r 的 幅 值 和 /或 方向 在 参考 坐标 系 B 取决 于 一 个 比例 变量 9， 那么 r 被 称 为 
在 坐标 系 B 中 变量 0 的 矢量 函数 。 矢 量 r 可 以 是 在 一 个 参考 坐标 系 中 的 一 个 变量 的 函数 ， 但 
是 在 男 一 个 参考 坐标 系 中 它 独立 于 这 个 变量 。 

例 1-2 参考 坐标 系 和 参数 相关 。 

在 图 1-15 中 ，P 表示 一 个 能 在 一 个 圈 内 外 自由 移动 的 点 ， 它 由 3 个 旋转 关节 连 杆 构成 ， 
如 果 所 示 的 角度 为 9?、 pgp、 yy， 那么 在 坐标 系 G(X，Y) 内 矢量 r 是 变量 9、 pgp、y 的 一 个 
矢量 函数 。 矢 量 r 的 长 度 和 方向 取决 于 变量 0、 p, go 
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=X 





图 1-15 全 局 坐标 系 G(X，Y) 中 平面 3R 机 械 手 和 末端 点 P 的 位 置 矢量 
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如 果 G(X，Y) 和 BCz， y) 指 定 连接 至 地 面 和 连 杆 的 参考 坐标 系 ,， 点 已 是 如 图 1-16 


所 示 的 连 杆 3 的 末端 点 ， 那 么 点 了 的 
数 ， 但 是 独立 于 变量 9。 


位 置 矢 量 r 在 参考 坐标 系 B 中 就 是 变量 p、 PREK 











=X 





图 1-16 在 第 2 个 连 杆 上 所 建立 的 局 部 坐标 系 B(x， y) 中 平面 3R 机 
械 手 和 末端 点 PP 的 位 置 矢量 





1.5 机 器 人 动力 学 问题 








有 3 个 基本 的 系统 方法 可 用 来 描述 机 械 手 连 杆 的 相对 位 置 和 方向 。 第 1 种 也 是 机 器 人 运 
动 学 中 最 稼 用 的 方法 就 是 用 于 定义 空间 机 构 的 D-H 法 和 点 的 齐 次 变换 。 利 用 4X4 和 矩阵 或 者 
齐 次 变换 描述 点 矢量 的 空间 变换 。 在 机 器 人 学 中 ， 该 矩阵 用 于 描述 相对 另 一 个 坐标 系 的 坐标 
系 。 变 换 和 矩阵 法 是 描述 机 器 人 运动 的 最 常用 方法 。 

机 器 人 运动 学 的 研究 人 员 试 着 用 另 一 种 方法 来 描述 刚体 变换 ， 刚 体 变换 法 是 基于 数学 家 
和 物理 学 家 所 介绍 的 概念 的 ， 例 如 螺旋 理论 (screw theory)、 李 代数 (Lie algebra) 以 及 & 
代数 (Epsilon algebra). 。 相 对 于 参考 坐标 系 的 刚体 变换 或 者 坐标 系 变 换 可 用 螺旋 位 移 表 述 ， 
螺旋 位 移 是 关于 同一 旋转 轴 的 旋转 角 的 一 个 变换 。 虽 然 可 以 成 功 地 利用 螺旋 理论 和 李 代 数 分 
析 机 器 人 ， 但 是 分 析 结 果 最 终 应 该 用 和 矩阵 表述 。 









































1.6 主题 预览 






































本 书 共 分 为 3 个 部 分 : 第 工 部 分 为 运动 学 ， 第 下 部 分 为 动力 学 ， 第 于 部 分 为 控制 技术 。 


其 中 第 工 部 分 是 最 重要 的 部 分 ， 因 为 





它 定义 和 描述 了 机 器 人 分 析 的 基础 规则 和 相关 工具 。 











刚体 旋转 分 析 是 坐标 系 相对 运动 分 析 的 主体 。 它 是 关于 怎样 描述 相对 于 男 一 个 坐标 系 的 
坐标 系 方向 的 。 在 第 2 章 和 第 3 章 中 ， 定 义 和 描 述 了 有 具有 共同 原点 坐标 系 的 旋转 运动 学 。 
此 ,第 2 章 和 第 3 章 是 关于 两 个 通过 旋转 关节 的 直接 相连 连 杆 的 运动 的 。 坐 标 系 原 点 相对 彼 











此 能 够 移动 。 第 4 章 是 关于 两 个 非 直 3 




















接连 接连 杆 的 运动 的 。 





在 第 5 章 中 ， 利 用 刚体 连 杆 的 位 置 和 方向 运动 学 系统 地 描述 在 全 局 笛 卡 儿 坐标 系 中 机 顺 








人 终端 连 杆 的 配置 ， 这 称 为 正 向 运动 学 。 在 正 向 运动 学 中 我 们 有 兴趣 找到 基于 所 测 得 的 关节 
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坐标 的 末端 执行 器 配置 。D-H 法 是 正 向 运动 学 中 的 主要 工具 。 在 第 5 章 中 ,我 们 将 说 明 在 
运动 学 上 怎样 将 一 个 机 器 人 解构 为 具有 1 个 或 两 个 自由 度 的 基本 机 构 ， 在 运动 学 上 怎样 将 基 
本 的 机 构 构 建成 一 个 任意 机 器 人 。 

第 6 章 介 绍 了 从 笛 卡 儿 侍 标 系 到 关节 空间 的 机 器 人 运动 学 ， 该 机 器 人 运动 学 称 为 逆向 运 
动 学 。 我 们 从 一 个 已 知 的 末端 执行 器 位 置 和 方向 开始 ， 寻 求 一 组 合适 的 关节 坐标 。 

机 器 人 刚性 连 杆 间 的 速度 关系 是 第 7 章 和 第 8 章 的 主题 。 第 7 章 介 绍 了 角速度 矢量 和 和 角 
速度 矩阵 的 定义 。 第 8 章 介绍 了 机 器 人 连 杆 之 间 以 及 在 关节 空间 和 笛 卡 儿 空 间 中 的 微分 运动 
之 间 的 关系 。 雅 可 比 和 矩阵 〈Jacobian matrix) 是 第 8 章 的 主要 概念 。 

在 第 9 章 ， 介 绍 了 有 效 的 可 用 方法 ， 即 机 器 人 中 容易 计算 机 化 的 计算 。 

第 工 部 分 包括 机 器 人 运动 学 中 的 应 用 数值 法 。 

在 第 工 部 分 中 ， 讲 述 如 何 确定 机 器 人 运动 方程 的 技术 。 这 部 分 以 第 10 章 中 相关 连 杆 的 
加 速度 分 析 开 始 。 第 11 章 中 介绍 和 推导 了 机 器 人 运动 方程 。 其 中 拉 格 天 日 法 (Lagrange 
method) 是 动力 学 的 主要 方法 ， 牛 顿 - 欧 拉 法 作为 蔡 换 工具 ， 以 寻求 运动 方程 。 此 外 欧 拉 - 拉 
格 朗 日 (Euler-Lagrange) 法 是 一 个 比较 简单 的 概念 ， 然 而 它 提 供 无 须 的 内 部 关节 力 。 另 一 
方面 ， 拉 格 朗 日 法 是 比较 系统 的 ， 它 为 计算 机 计算 提供 了 基础 。 

在 第 部分， 从 一 个 简单 的 路 径 分 析 描 述 开 始 。 然 后 ， 利 用 浮 点 时 间 法 描述 机 器 人 最 优 
控制 。 浮 点 时 间 技 术 所 要 求 提供 的 转 和 矩 ， 可 以 使 机 器 人 在 一 个 开 环 控制 中 沿 着 预定 的 运动 路 
径 运 动 。 为 了 弥补 预期 的 运动 学 与 实际 运动 学 之 间 的 可 能 误差 .我 们 解释 了 计算 转 和 矩 控制 法 
和 闭环 控制 算法 的 概念 。 

























































































1.7 机 器 人 一 一 多 学 科 交 叉 的 机 器 





我 们 注意 到 ， 机 器 人 机 械 结构 是 机 器 人 中 唯一 可 见 的 部 分 。 机 器 人 学 涉及 多 学 科 领 域 。 
在 该 领域 中 ,来 自 不 同 专业 的 工程 师 均 起 着 重要 的 作用 〈 如 机 械 工程 师 、 系 统 工 程 师 、 电 气 
工程 师 、 电 子 工 程 师 和 计算 机 科学 工程 师 )。 因 此 ， 对 于 机 需 人 工程 师 来 说 ， 他 们 基本 上 要 
能 够 理解 所 涉及 的 其 他 学 科 的 主要 概念 ， 并 且 与 这 些 学 科 的 工程 师 进 行 交流 沟通 。 























1.8 本章 小 结 





有 两 种 机 器 人 ， 分 别 为 ， 串联 机 器 人 和 并 联机 器 人 。 串 联机 器 由 系统 刚性 连 杆 构成 ， 每 
对 连 杆 由 旋转 关节 R 或 者 棱柱 平 动 关节 P 相连 接 。R 或 者 P 关节 仅仅 只 能 提供 一 个 自由 度 ， 
R 或 者 P 关节 分 别 是 旋转 的 或 者 移动 的 。 机 器 人 终端 连 杆 ， 也 称 之 为 末端 执行 器 ， 是 机 顺 
人 中 与 环境 相互 作用 的 操作 元 件 。 

在 机 器 工作 空间 中 ， 为 了 达到 任何 期 望 方向 的 点 ,一 个 机 器 人 需要 至 少 有 6 个 自由 度 。 
因此 ， 它 必须 至 少 有 6 个 连 杆 和 6 个 关节 。 大 部 分 机 器 人 使 用 3 个 自由 度 定位 手腕 位 置 ， 使 
用 其 他 的 3 个 自由 度 定向 位 于 手腕 点 上 的 末端 执行 器 。 
我 们 将 笛 卡 儿 坐 标 系 与 机 器 人 每 个 连 杆 联系 起 来 ， 并 且 确 定 每 个 坐标 系 相 对 于 其 他 坐标 
系 的 位 置 和 方向 。 因 此 ， 为 了 决定 末端 执行 器 的 位 置 和 定向 ， 我 们 需要 寻求 基 座 坐标 系 〈 参 
考 坐 标 系 ) 中 的 末端 执行 器 坐标 系 。 









































和 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





习题 


1-1 


位 置 矢量 的 含义 。 


a 4 、 G 了 A N 
如 果 r EMERE., 解释 6r p ,Brp 和 Crp 的 含义 。 


1-2 








速度 矢量 的 含义 。 


x s ` 了 B AN 
如 果 w 是 速度 矢量 ， 解 释 SwupP Sop 和 Bop 的 含义 。 


1-3 


角速度 矢量 的 含义 。 











> s 2 G B B2 AN 
如 果 @ 是 角速度 矢量 ， fe FE ob， +p ØB: +6 Op Mg, OB 的 含义 。 


1-4 


变换 矩阵 的 含义 。 














如 果 了 PERE, IS ATER Te, AIST, 的 含义 。 


1-5 


机 器 人 规则 。 


机 器 人 规则 0 和 规则 1 的 区 别 是 什么 ? 


1-6 





机 器 人 学 的 新 规则 。 





如 果 将 第 4 条 规则 添加 到 机 器 人 学 中 ， 例 如 “机 器 人 必须 保护 其 他 机 器 人 ， 只 要 这 样 的 
保护 不 与 更 高 的 规则 发 生 冲 突 ?， 你 是 怎样 认为 的 ? 








1-7 “robot” 词 语 。 

给 出 “robot” 词 语 的 原意 和 含义 。 

1-8 机 器 人 的 分 类 。 

你 可 以 将 起 重 机 作为 一 种 机 器 人 进行 考虑 吗 ? 
1-9 机 器 人 市 场 。 





大 部 分 小 的 机 器 人 制造 商 于 1990 年 从 市 场 上 消失 了 ， 仅 有 一 些 大 公司 存活 了 下 来 。 你 
认为 为 什么 会 产生 这 样 的 结果 ? 

1-10 类 人 机 器 人 或 者 称 为 人 型 机 器 人 ) 

移动 机 器 人 工业 正在 尽力 地 使 机 器 尽 可 能 地 类 似 于 人 。 你 认为 其 中 的 原因 是 什么 (“人 有 
最 好 的 结构 设计 ， 人 有 最 简单 的 设计 ,或 者 这 些 种 类 的 机 器 人 在 市 场 上 比较 好 销售 )? 


1=11 








Dla ASL A CT BL BOR BAO 





你 认为 为 什么 我 们 将 具有 机 械 地 工作 或 者 行为 并 且 没 有 情绪 通常 对 无 问题 的 指令 做 出 响 
应 的 人 称 为 机 器 人 式 的 人 ? 

1-12 机 器 人 期 刊 。 

给 出 与 机 器 人 技术 相关 的 10 个 技术 期 刊 名 称 。 


1-13 
搜索 机 器 人 文献 ， 给 出 当前 美国 、 欧 洲 、 日 本 工业 机 器 人 的 应 用 大 约 数量 。 





工业 中 机 器 人 的 数量 。 








出 


1-14 机 器 人 国家 。 
搜索 机 右 人 文献 ， 依 据 工 业 机 右 人 的 使 用 数量 给 出 排名 第 1、 第 2 和 第 3 的 国家 。 


1-15 














机 器 人 的 优点 和 缺点 。 








tL RAL ae A SCHR. DL LAR AY 10 个 优点 和 两 个 缺点 。 
1-16 HLA ANALAE A. 
为 什么 我 们 不 能 用 机 器 人 取代 每 一 种 机 械 机 构 〈 例 如 装配 线 中 的 机 械 机 构 等 )? 机 器 人 
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正在 取代 机 械 机 构 吗 ? 

1-17 高 副 和 低 副 。 

关节 可 以 分 为 低 副 和 高 副 。 给 出 机 械 学 中 高 副 和 低 副 的 含义 。 

1-18 自由 度 的 消除 。 

关节 依据 所 提供 的 自由 度数 进行 分 类 ， 或 者 依据 所 消除 的 自由 度 进 行 分 类 。 因 此 有 5 类 
关节 。 说 出 每 类 中 关节 的 名 称 和 数量 。 

1-19 人 的 手腕 自由 度 。 

观察 自己 手腕 的 自由 度数 。 

1-20 人 手 是 一 个 宛 余 机 械 手 。 

手臂 (包括 肩膀 、 肘 部 和 手腕 ) AT 个 自由 度 。 在 我 们 手中 ， 相 对 于 6 A 
个 外 加 自由 度 的 优点 是 什么 ? 

1-21 多 自由 度 机 器 人 。 

有 时 我 们 不 需要 一 个 太 多 自由 度 的 机 器 人 来 完成 由 低 自 由 度 机 器 人 所 做 的 具体 工作 。 你 
是 怎样 看 待 可 变 自由 度 机 器 人 的 ? 

1-22 TURAL HHA. 

讨论 元 余 机 械 手 的 可 能 应 用 。 

K1-23 欧 几 里 得 空间 中 独立 的 笛 卡 儿 坐 标 系统 。 

在 三 维 欧 几 里 得 空间 中 ， 我 们 需要 一 个 三 元 数 定位 一 个 点 。 有 两 个 独立 的 非 芭 加 的 三 元 
数 。 在 四 维 欧 几 里 得 空间 中 ， 可 以 想象 一 下 有 多 少 种 非 受 加 的 笛 卡 儿 坐 标 系 统 ? E n 维 欧 
几 里 得 空间 中 ， 我 们 有 和 多少 种 笛 卡 儿 坐 标 系统 ? 

克 1-24 非 正 交 三 元 数 的 缺点 。 

为 什么 我 们 使 用 一 个 正 交 的 三 元 数 定 义 一 个 笛 卡 儿 空 间 ， 能 用 非 正 交 三 元 数 定义 一 个 三 
作 空 间 吗 ? 

Wi1-25 正 交 三 元 数 的 用 处 。 

正 交 是 所 有 有 用 坐标 系统 的 共同 特性 ， 例 如 笛 卡 儿 坐 标 系统 、 圆 柱 坐 标 系统 、 球 形 坐 标 
系统 、 抛 物 线 坐 标 系统 、 椭 球 坐 标 系统 。 为 什么 我 们 仅仅 只 定义 和 使 用 正 交 坐标 系统 ?基于 
内 积 和 坐标 系统 的 单位 矢量 ， 例 如 

r=(r»iji+(r 7) 二 Or) 

你 认为 定义 矢量 的 能 力 是 定义 正 交 坐标 系统 的 主要 原因 吗 ? 

W1-26 3 EHRE., 

说 明 : aes Xb c 一 0， 那么 矢量 a 、 b, c 是 共 面 的 。 
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REP Mir e es 量 而 被 定义 。 














r=r (t) 
描述 矢量 变量 的 矢量 函数 和 标量 函数 的 含义 ， 并 给 出 一 个 矢量 函数 例子 。 
矢量 变量 的 矢量 函数 : 

a =a (b) 
矢量 变量 的 标量 函数 

f=f ob) 


ws: 运动 学 、 动 力学 与 控制 技术 





W1-28 关联 坐标 系 和 独立 坐标 系 。 

标量 变量 的 矢量 函数 是 一 个 关联 坐标 系 的 参量 。 矢 量变 量 的 矢量 函数 是 关联 坐标 系 吗 、 
有 关 矢 量变 量 的 标量 函数 呢 ? 

次 1-29 ”坐标 和 矢量 函数 。 

Wr EMERE, v EREKE, vr) 表示 w 是 r 的 函数 ， 解 释 Bvw pEr p). 


















































第 | 部 分 
Dla Am 


运动 学 是 几何 学 在 运动 方面 的 应 用 。 它 仅 限 于 运动 位 置 、 方 向 及 其 时 间 导 数 的 纯粹 几何 
描述 。 在 机 需 人 学 中 ， 利 用 机 械 臂 及 其 分 派 任务 的 运 动 学 描述 建立 起 动力 学 和 控制 的 基本 方 
程式 。 

因为 在 机 器 人 系统 中 连 杆 和 机 械 臂 是 作为 刚体 建 模 的 ， 因 此 刚体 的 位 移 特 征 在 机 器 人 学 
中 必然 处 于 中 心 位 置 。 利 用 矢量 和 和 矩阵 代数 建立 一 个 系统 的 通用 的 方法 以 描述 机 械 辟 相对 于 
全 局 坐标 G 的 位 置 。 因 为 机 器 人 手臂 可 以 彼此 相对 旋转 或 移动 ， 因 此 沿 着 每 根 连 杆 的 关节 
轴 便 可 以 建立 起 刚体 坐标 系 A、B、C 等 或 者 Bl Bo. Bs 等 以 找到 它们 在 参考 坐标 系 G 内 
的 相对 配置 。 利 用 连 杆 坐标 系 之 间 的 坐标 变换 抢 阵 *Ta 可 以 定义 连 杆 B 相对 于 连 杆 A 的 


5, 


(an 





















































找到 刚体 坐标 系 B 相对 于 全 局 坐标 系 G AY ERE ES Te 便 可 以 简化 正 向 运动 学 问题 。 
用 3X3 旋转 矩阵 描述 局 部 坐标 系 相对 于 全 局 坐标 系 的 旋转 操作 。 引 入 齐 次 坐标 是 为 了 描述 
三 维 空间 中 的 位 置 矢量 和 方向 矢量 。 将 旋转 和 矩阵 扩展 成 包含 旋转 和 移动 的 4X4 齐 次 变换 算 
阵 。 用 一 套 特殊 的 规则 建立 起 能 够 表达 机 器 人 刚性 连 杆 的 齐 次 变换 抢 阵 ， 该 矩阵 在 Denavit 
和 Hartenberg (1955) 之 后 被 称 为 Denavit-Hartenberg 矩阵 (D-H HM). D-H 矩阵 的 优点 
就 在 于 对 机 器 人 连 杆 运动 方程 求 导 的 算法 通用 性 。 

刚体 位 移 解 析 描 述 的 基础 是 刚体 上 的 所 有 点 必须 保持 其 原始 的 相对 位 置 ， 无 论 刚体 的 新 
立 置 和 方向 如 何 。 总 的 刚体 位 移 总 能 简化 成 两 个 基本 部 分 的 和 : 固定 在 刚体 中 的 任意 参考 点 
的 移动 位 移 加 上 刚体 绕 着 通过 该 固定 点 轴线 的 独特 旋转 。 

对 刚体 位 移 运动 的 研究 可 以 找到 一 个 矢量 在 全 局 坐标 系 中 的 时 间 变 化 率 与 其 在 局 部 坐标 
系 中 的 时 间 变 化 率 之 间 的 关系 。 

从 局 部 坐标 系 B 到 全 局 坐标 系 G 的 变换 可 表述 为 

Sr =CRg’r +°d p 

这 里 ,ar 是 坐标 系 B 中 一 个 点 的 位 置 矢 量 ,Sr 是 同一 点 在 坐标 系 G 中 的 位 置 矢量 ,，d 是 刚体 
坐标 系 B(ozyz) 坐 标 原 点 o 相对 于 全 局 坐标 系 G(OXYZ ) 上 坐标 原点 O 的 位 置 拓 量 。 因 此 ， 变 
换 有 两 部 分 : 将 坐标 原点 o 移动 到 坐标 原点 O 处 的 移动 部 分 &， 将 坐标 系 B(ozyz) 的 坐标 轴 
变换 到 与 坐标 系 G(OX YZ ) 坐标 轴 相 一 致 的 旋转 部 分 SR o 
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可 以 将 变换 公式 sr = Rg”r 十 sd s 扩展 到 连接 两 个 以 上 的 坐标 系 。 将 坐标 系 Bi 变换 
到 坐标 系 B: 中 ， 再 将 坐标 系 Bo 变换 到 全 局 坐标 系 G 中 ， 其 综合 变换 公式 为 


Sr = (SRg: Br 十 da ) + (Rp Pr +” dp, ) 
=CRpg, Rp Br +dp, +® dp, 
=C Rp; Bip + dp, 


— plat Ade BA Hs AY n 根 刚性 连 杆 组 成 的 。 固 定 在 地 面 上 的 连 杆 是 连 杆 
(0)， 连 接 到 末端 移动 连 杆 即 末 端 执 行 占 的 连 杆 是 连 杆 (n)。 机 器 人 运动 学 分 析 中 有 两 个 重 
要 的 问题 : 正 向 和 运动 学 问题 和 逆向 和 运动 学 问题 。 

正 向 运动 学 问题 就 是 点 书 的 位 置 天 量 "rp 位 于 末端 执行 吉 坐 标 系 B, ， 我 们 需要 找到 点 
P 在 基体 坐标 系 Bo 中 的 位 置 矢量 "rp 。 正 向 运动 学 问题 就 是 已 知 各 关节 变量 的 值 求 取 末 端 
SAAT ae AY (i EE o 

道 向 运动 学 问题 就 是 已 知 点 了 在 基体 坐标 系 Bo 中 的 位 置 矢 量 "rp ， 寻 找 点 P 位 于 末端 
执行 噩 坐标 系 B, 中 的 位 置 矢量 "rp 。 这 个 问题 等 效 于 已 知 来 端 执 行 融 的 位 置 求解 机 顺 人 到 
达 点 了 的 一 组 关节 变量 。 

在 这 部 分 ， 我 们 将 探讨 有 关 从 一 坐标 系 变换 到 另 一 坐标 系 的 正 向 、 逆 向 运动 信息 的 变换 


ATEA 



























































对 于 一 点 固定 的 刚体 ， 绕 固定 点 旋转 是 刚体 唯一 可 能 的 运动 。 我 们 通过 刚体 坐标 系 B 
说 明 在 另 一 坐标 系 G 中 旋转 的 刚体 ， 如 图 2-1 所 示 。 我 们 建立 基于 变换 矩阵 的 旋转 微 积 分 
法 ， 以 确定 在 坐标 系 G 中 坐标 系 B 的 方向 和 在 两 个 坐标 系 中 刚体 上 的 点 卫 的 相关 坐标 。 








图 2-1 在 固定 的 全 局 坐标 系 G 中 使 刚体 坐标 系 B 绕 固定 点 O 旋转 











2.1 线 全 局 直角 坐标 轴 旋 转 


考虑 处 于 局 部 坐标 系 Ozyz 的 刚体 B， 局 部 坐标 系 Ory: 原始 状态 与 全 局 坐标 系 OXYZ 
重合 。 刚 体 B 上 的 点 O 固定 于 地 面 G， 并 且 点 O 也 是 两 个 坐标 系 的 原点 。 如 果 刚 体 B 绕 全 
局 坐标 系 的 Z 轴 旋 转 a 角 ， 在 局 部 坐标 系 和 全 局 坐标 系 中 刚体 上 任何 点 了 的 坐标 通过 下 面 

方程 相互 联系 为 
F =Qza r (2-1) 


X x 
=|Y Br 一 | y (2-2) 
Z z 


C) 


这 里 


以 及 


ws: 运动 学 、 动 力学 与 控制 技术 











ca —sa 0 
Qz |: ca 0 (2-3) 
0 0 1 
类 似 地 ， 在 全 局 坐标 系 中 绕 Y Hee BA. BEX 轴 旋 转 y 角 与 点 P 的 局 部 和 全 局 坐标 
通过 下 面 方程 相互 关联 为 
了 一 Ov.eBr (2-4) 
了 一 COx.yBr (2-5) 
这 里 
c 0 sp 
Qy.s=| 0 1 : (2-6) 
—sB 0 cf 
1 0 0 
Qx.y=|0 cy z (2-7) 
0 sy cy 
证 明 : 





假设 (i， js LMC, J, KAIERA Ozyz 和 坐标 系 OXYZ 沿 着 各 自 坐标 轴 
的 单位 矢量 。 刚 体 有 一 个 空间 固定 点 O ， 该 点 是 坐标 系 Ozyz 和 坐标 系 OXYZ 的 共同 原点 。 
图 2-2 所 示 为 当 全 局 和 局 部 坐标 系 重合 时 点 P 的 位 置 矢量 。 











x 


=X 











K 2-2 当 全 局 和 局 部 坐标 系 重合 时 点 了 的 位 置 矢 量 





A P 的 初始 位 置 示 为 P1。P1 的 位 置 撩 量 rj 可 以 在 刚体 (局 部 ) 坐标 系 和 全 局 坐标 系 














中 表述 为 
Br =x] ityijtzik (2-8) 
Gp =X I+Y, +Z, R (2-9) 
xı =X 
yi=Vi (2-10) 
21=Z) 
RP, Br 指 在 刚体 坐标 系 BP eA i Retr, ,Sr 指 在 全 局 坐标 系 G 中 所 表达 的 位 置 
RET o 
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如 果 刚 体 绕 全 局 坐标 系 的 Z 轴 旋 转 a ffi. 那么 当局 部 坐标 系 B 绕 2 轴 旋 转 后 点 了 的 位 
置 矢量 如 图 2-3 所 示 。 现 在 点 Ps 的 位 置 拓 量 ro 在 两 个 坐标 系 中 表达 如 下 : 











=X 





图 2-3 当局 部 坐标 系 也 绕 Z 轴 旋 转 后 点 P 的 位 置 矢 量 


Br, =x; t yj tzok 


Gp, =X: +Y: +Z:K 
依据 点 积 和 式 (2-11)， 我 们 可 以 写 出 : 
I be r=! *x itl ys tl 本 zok 
Y:=f e=] sa it] “yoo tl *zok 
Kem=K- x ei +K 。 yop +K 5 zok 
I+k T2 
Y: |=| f.i Jeg Sek] ys 
Z? R.i K+ej Kek 


之 2 


(2-11) 
(2-12) 


(2-13) 
(2-14) 
(2-15) 


(2-16) 


Se Z 轴 的 旋转 和 矩阵 Qz,s 中 的 元 素 称 为 相对 于 坐标 系 OXYZ 的 矢量 Br, 的 正 向 余弦 。 图 2- 
4 所 示 为 在 两 个 坐标 系 Ozyz 和 OXYZ 中 矢量 r 初始 的 和 最 终 的 配置 。 分 析 图 2-4 可 知 : 





=X 











图 2-4 局 部 坐标 系 绕 全 局 坐标 系 的 Z 轴 旋 转 前 后 点 了 的 位 置 矢量 
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I+i=cosa, 了. 7) 一 一 sina， [I +k=0 
J +i=sina, J *j7=cosa, J .k=0 (2-17) 
R .f=0, K .7 一 0， K .k=1 





综合 式 (2-16) 与 式 (2-17) 得 知 ， 通 过 Z 轴 旋 转 和 矩阵 Qz,s 与 矢量 Sr, 的 乘积 我 们 能 够 








求 得 矢量 cr, 的 坐标 分 量 。 
X2 cosa —sina 0) [72 
Yo 区 cosa 0] | ye (2-18) 
Zo 0 0 1 Ze 
也 可 表达 为 下 面 的 式 (2-19): 
Cr, =Q z.a 7 (2-19) 
这 里 
cosa —sina 0 
ae cosa 0 (2-20) 
0 0 1 








式 (2-19) 表明 ， 在 全 局 坐标 系 中 第 2 位 置 的 矢量 r 等 于 旋转 矩阵 Qz 乘 以 在 局 部 坐标 
系 中 的 位 置 矢 量 。 因 此 ， 在 刚体 绕 2 轴 旋 转 后 ， 如 果 知 道 它 的 局 部 坐标 系 坐 标 ， 我 们 就 能 
够 找到 其 上 一 点 的 全 局 坐标 。 

类 似 地 ， 绕 Y 轴 旋 转 B8 角 、 绕 XX 轴 旋 转 y 角 可 分 别 用 绕 Y 轴 的 旋转 矩阵 Qy.p 和 绕 X Ah 
的 旋转 矩阵 Qx,y 表 示 。 




















cosB 0 sing 
Qy.s=| 0 1 0 (2-21) 
一 Sin8 0 cosB 
1 0 0 
Qx.y=|0 cosy —siny (2-22) 
0 siny cosy 








旋转 和 矩阵 0z,。、QyY,s 和 Qx,y 被 称 为 基本 全 局 旋转 和 矩阵。 通常 指 通 过 a、 8、 VIE 
全 局 坐标 轴 的 第 1、 第 2 和 第 3 旋转 。 
例 2-1 绕 全 局 坐标 轴 连 续 旋转 。 








图 2-5 平板 在 初始 位 置 处 顶点 Pi 的 坐标 
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如 图 2-5 所 示 ， 平 板 顶 点 P(5， 30, 10) 绕 2 轴 旋 转 30"， 接 着 绕 X 轴 旋 转 30"， 最 后 
Se Y 轴 旋 转 90"， 求 取 平 板 顶 点 的 最 后 位 置 。 我 们 可 以 先 通过 旋转 矩阵 Qz,se 和 平板 顶点 P 
(5, 30, 10) 相 乘 找 到 第 1 次 旋转 后 在 新 的 全 局 位 置 矢 量 为 




















X2 cos 30° —sin30° 0) (5 —10. 68 
Y |=] sin30° cos30” 0|130|=| 28.48 (2-23) 
Z2 0 0 i. 10 10. 0 
然后 利用 旋转 矩阵 Ox 30° MEREL 10.68 28.48 10)" 相 乘 可 以 获得 点 P 在 第 2 
次 旋转 后 的 位 置 矢量 为 

X3 1 0 0 — 10. 68 — 10. 68 

Ys |=|0 cos 30” 一 Sin 3 | 28. | | 19. 66 (2-24) 

Z3 0 sin30° cos 30° 10. 0 22.9 








最 后 利用 旋转 矩阵 Oy,%* 和 位 置 矢 量 [ 一 10.68 19.66 22.9] 相 乘 获得 平板 顶点 P 在 
第 3 次 旋转 之 后 的 最 终 位 置 。 平 板 和 点 也 在 第 1 次 、 第 2 次 、 第 3 次 和 第 4 次 的 位 置 如 图 


2-6 所 示 。 
Xa cos 90° 0 —sin 90°) (—10. 68 22. 90 
Yu | 0 1 0 | | i= 66 
Za sin90° 0 cos 90° 22.9 10. 68 





(2-25) 














图 2-6 MA P 和 平板 分 别 在 第 1 次 、 第 2 次 、 第 3 次 和 第 4 次 的 位 置 





例 2-2 与 时 间 关 联 的 全 局 旋转 。 
考虑 刚体 B。 刚 体 以 0. 3rad/s 的 角速度 在 坐标 系 G 中 绕 Y 轴 连 续 旋 转 。 刚 体 旋转 变换 
矩阵 为 





0 1 0 
一 Sin0. 3! 0  cos0. 3t 


69,,= (2-26) 





cos0.3t 0 sin0. | 


刚体 B 上 的 任何 点 ， 绕 着 旋转 半径 为 R= 二 VX? 十 Z? 的 圆 移 动 ， 该 圆 平行 于 平 
M(x, Z). 
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X cos0.31t 0 sind. 3t) (x xcos0. 3t+zsin0. 3t 
Y |= 0 1 0 y | 二 y (2-27) 
Z 一 Sin0. 3 上 0 cos0. 3 z zcos0. 3t— xsin0. 3t 


X? +Z? = (xcos0. 3t+zsin0. 3t)? + (zcos0. 3t —zxsin0. 3t)? =x? +z? =R? (2-28) 
考虑 在 矢量 ar =(1 0 0]' 处 的 一 点 P。 在 1 二 1s 时， 该 点 将 在 如 下 坐标 处 被 看 到 




















X cos0.3 0 sin0.31 (1 0. 955 
Y |= 0 1 0 [| 一 0 (2-29) 
Z 一 Sin0.3 0 cos0.3/) NO 一 0. 295 
i 二 2s 时 ， 该 点 位 置 为 
X cos0.6 0 sin0.61 (1 0. 825 
Y |= 0 1 0 0 | 二 |0.564 (2-30) 
Z 一 Sin0.6 0 cos0.6) NO 0 
通过 将 下 面 方程 对 时 间 求 导 ， 我 们 能 够 获得 刚体 中 点 P 的 全 局 速度 : 
Gr, =Ov eh (2-31) 
cos0. 3f 0 sin0. 3¢ 
Qy,s= 0 1 0 (2-32) 


—sin0.3t 0 cos0. 3t 


因此 ， 它 的 速度 矢量 全 局 表达 式 为 





zcos0. 3f—asin0. 3t 
Suop 一 Or.sarp 一 0.3 0 (2-33) 
—xcos0. 3t—zsin0. 3t 


例 2-3 全 局 旋转 ， 局 部 位 置 。 








图 2-7 在 例 2-3 中 点 P 旋转 前 后 的 位 置 





点 卫 绕 2 轴 旋 转 60" 后 如 果 移 动 到 位 置 拓 量 Sr, 二 (4 3 2)7 处 ， 它 在 局 部 坐标 系 中 的 
位 置 矢量 为 




















Br, =Q; hoe oP (2-34) 
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T? cos 60° —sin60° 0\) 1/4 4.6 
yo |=| sin60° cos60” 0 3}=]—-1.95 
i 0 0 i) ke 2.0 


在 旋转 之 前 ， 局 部 坐标 系 B 与 全 局 坐标 系 G 重合 ， 这 样 一 来 在 旋转 之 前 已 点 的 全 局 坐 
标 也 是 ?ri 二 (4.60 一 1.95 2.0)1。 点 了 在 旋转 前 后 的 位 置 如 图 2-7 所 示 。 








2.2 绕 全 局 直角 坐标 轴 连 续 旋 转 





绕 全 局 坐标 轴 先 后 进行 一 系列 旋转 QO1 Q2 Qs. ot. 0Q4, 后 ， 具 有 位 置 矢量 7 的 刚 

体 上 一 点 P 的 最 终 全 局 位 置 拓 量 可 用 式 (2-35) 求 得 : 
cr =°Q,"r (2-35) 

这 里 CO =Qn Q320: (2-36) 

Sr 和 Br 分 别 表示 全 局 坐标 系 和 局 部 坐标 系 中 的 位 置 矢量 r ;0 被 称 为 全 局 旋转 和 矩阵 ， 
它 将 局 部 坐标 转换 成 其 相应 的 全 局 坐标 。 
由 于 和 矩阵 相 乘 不 能 交换 ， 因 此 其 旋转 顺序 是 很 重要 的 。 旋 转 矩 阵 是 正 交 的 ， 那 么 它 的 转 
AEM QT SPE AY wR Qo! 























Q'=Q"! (2-37) 
绕 全 局 坐标 轴 旋 转 在 概念 上 是 很 简单 的 ， 因 为 旋转 轴 在 空间 固定 。 假 设 全 局 坐标 系 中 刚 
体 上 每 一 点 的 坐标 最 初等 于 其 局 部 坐标 。 刚 体 绕 全 局 坐标 轴 旋 转 ， 正 确 的 全 局 旋转 矩阵 给 我 
们 一 个 新 的 全 局 坐标 。 第 1 次 旋转 后 ， 当 我 们 找到 刚体 所 有 点 的 坐标 时 ， 在 第 2 次 旋转 之 前 
的 状况 与 第 1 次 旋转 之 前 的 状况 类 似 。 
Gil 2-4 连续 的 全 局 旋转 矩阵。 
旋转 Qz,。、Qy,g 和 Qx,y 之 后 ,全 局 旋转 矩阵 为 
ca cp — cBsa sp \e 
| (2-38) 











°Q,=Qx.yOy.pQz,..=|cYsatcasBsy cacy—sasBsy —cBsy 
sasy—cacysß casy+cYsasB cBey 
Gl 2-5 连续 的 全 局 旋转 ， 全 局 位 置 。 

机 械 辟 末端 点 P 如 图 2-8 所 示 ， 位 置 为 











7 
Xi 0 0 0.0 \ 
Yı |=] lcos@ |=} 1X cos75° |=] 0. 26 | (2-39) 
Z\ Zsin@ 1X sin75° 0. 97 

















先后 分 别 绕 X Mee — 29°, Be Z 轴 旋 转 30°. 
再 次 绕 X 轴 旋 转 132 后 ， 末 端点 新 位 置 的 旋转 矩阵 
可 用 式 (2-40) 求 得 : 
SQ, =Qx.1320z.300 x.—29 
0. 87 —0;44 —0.24 
区 33 —0.15 =Q; | (2-40) 


0. 37 0.89 —0.27 








图 2-8 fi) 2-5 中 的 机 械 辟 


























© ca 一 cosae 、cp8 一 cos8、cy 一 cosy 、sa 一 sina 、s8 一 sin8、sy 一 siny， 全 书 同 。 
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其 新 位 置 为 
X2 0.87 一 0.44 —0. 24)\/0.0 —=0n35 
Y2 H 39 —0.15 —O0. 中 | 94 | (2-41) 
Zo 0. 37 0.89 —0.27/\0. 97 —0. 031 
Gil 2-6 12 个 独立 的 3 次 全 局 旋转 。 
在 绕 全 局 坐标 轴 进 行 一 系列 旋转 之 后 ， 考 虑 在 最 终 方向 上 的 刚体 。 刚 体 坐 标 系 B 最 初 
与 全 局 坐标 系 G 重合 ， 如 果 两 个 相应 旋转 没有 绕 着 同一 轴 的 话 ， 那 么 我 们 仅仅 只 是 通过 绕 
全 局 轴 做 3 次 旋转 ， 便 可 将 刚体 坐标 系 B 变换 至 任何 的 最 终 方向 。 总 之 ， 有 12 种 不 同 的 绕 
全 局 坐标 轴 进 行 3 次 旋转 所 形成 的 独立 变换 和 矩阵。 它们 分 别 是 : 
Ox,yQy.pQ0z.0 
Oy .yOz.pOx.0 
Qz. yQ x. pQy.a 
QOz.yOy.pOx.0 
Oy,yOx,pQ0z.0 
QOx.yOz.pQy.a 
Qx.7Qy.pQx.0 
Oy.yOz.pQY.0 
O7.7yOx.p0z,0 
Ox.y¥Oz.pQX.0 
QY. vQ x.pQY.a 
Qz. yQY.BQ Z,a 
这 12 种 全 局 坐标 轴 的 3 次 旋转 的 扩展 形式 见 附 录 A。 
例 2-7 Te FE OFF A BE HA FR I OCI - 
改变 全 局 旋转 矩阵 的 次 序 等 效 于 改变 旋转 的 次 序 。 刚 体 B 上 一 点 己 的 位 置 位 于 位 置 矢 
量 arp=(1 2 3)7T 处 。 先 绕 X 轴 旋 转 30" 再 绕 Y 轴 旋 转 45" 后 ， 其 全 局 位 置 矢量 为 


0.53 一 0.84 0.13 \/1 一 0. 76 
(Srp)1—=QyY,.s45Qx,30° rp=| 0.0 0.15 0.99 ||2 3:27 (2-43) 
3 


一 0.85 —0.52 0.081 —1. 64 
如 有 果 我 们 改变 旋转 次 序 ， 其 位 置 矢量 应 为 


0.53 0.0 0.85 0 3. 08 
(Srp )2=Qx.30Ovus rp =|—0.84 0.15 0.52 || 2|=| 1.02 (2-44) 
3 


一 0.13 —0.99 0.081 —1. 86 

















(2-42) 


















































点 也 的 这 两 个 最 终 位 置 相距 : d= | (rp )1— (Grp )2 | 一 4. 456. 
太 例 2-8” 绕 全 局 坐标 轴 重 复 旋转 。 
如 果 刚 体 坐 标 系 也 绕 全 局 坐标 轴 X 轴 旋 转 y 弧度 ， 则 有 


_2n 





nEN (2-45) 


Q 
n 

















这 时 ， 我 们 需要 重复 n 次 ， 以 便 使 刚体 返回 至 其 最 初 的 位 置 。 我 们 可 以 通过 乘 以 Qx.。 
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并 检查 刚体 ， 直 至 我 们 获得 相同 的 和 矩阵。 因此 ， 刚 体 举 标 系 B 上 的 任何 点 将 被 转换 至 全 局 
坐标 系 中 的 相同 点 。 为 了 说 明 这 个 ,我们 可 以 对 矩阵 Qx,s 取 m URE. FR 























1 0 0 m 1 0 0 
0 0 ii Qn . Qn ( 2) : ( 2x) 
m ; _|I0 cos— 一 Sn 一 | _|0 cos}m 一 —sin 一 
QOy.,=|9 cosy —siny| = n n = n n 
0 siny cosy _ Qn Qn . ( 2x) ( 2) 
0 sin — cos 一 0 sin|m 一 cos| m 一 
n n n n 
(2-46) 
WR m=n. eA FR TZ 1H BI AB F EY E RE : 
1 0 0 
2x p ( 2m) 1 0 0 
Q" = cos| n F sin| n z lo 1 0 (2-47) 
Qn | ) 0 0 1 
0 Siml72 一 cos| 2 — 
n 
绕 着 任何 其 他 的 全 局 坐标 轴 重 复 旋 转 ， 也 能 得 到 相同 的 结果 。 
现在 让 我 们 绕 着 两 个 全 局 坐标 轴 旋 转 刚体 坐标 系 B， 例 如 绕 Z 轴 旋 转 a 角 ， 接 着 绕 X 








轴 旋 转 y 角 ， 则 有 








a=, yas {nisn} EN (2-48) 
我 们 可 以 设想 : 合成 旋转 重复 n=n Xn: 次 后 ， 将 刚体 坐标 系 也 返回 至 其 最 初 的 位 置 。 
(Qx.y Qz.a) nxn =I (2-49) 
作为 一 个 例子 ， 考 虑 一 至 和 7y 一 至 。 我们 需要 使 用 13 次 合成 旋转 才能 到 达 其 最 初 
位 置 
= 0.45 —0. 86603 0 
CO =Q x. QZ = 0 0 一 二 0 (2-50) 
0. 86603 =D 0 
0. 9997 “一 0.01922 ”一 0.01902 
GQF = 0. 01902 0. 99979 —0.0112 | 守 I (2-51) 
0. 01922 0. 01086 0. 9998 





如 果 ny 和 ns 有 一 个 公 因 子 ， 则 可 以 通过 一 个 较 低 的 旋转 次 数 将 刚体 坐标 系 B 返回 至 
其 最 初 位 置 。 例 如 z =n: 二 4， 我 们 仅仅 只 需要 应 用 合成 旋转 变换 矩阵 3 次 。 对 于 一 般 情 
况 ， 求 取 将 一 般 的 合成 旋转 变换 矩阵 "@ 返 回 至 其 最 初 位 置 的 重复 次 是 一 个 无 解 问题 。 








Q= [| Qx,s i=1,2,3 (2-52) 
j=l 
2 
aj=— m,n; EN (2-53) 
Nj 
co} =I n=? (2-54) 
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2.3 4a Ae fi. MN. if 


绕 全 局 坐标 系 X 轴 的 旋转 被 称 为 翻滚 (roll)， 绕 全 局 坐标 系 Y 轴 的 旋转 被 称 为 俯仰 
(pitch)， 绕 全 局 坐标 系 Z 轴 的 旋转 被 称 为 偏 航 (yaw)。 全 局 翻滚 -信仰 - 偏 航 和 矩阵 : 
图 2-9 所 示 为 关于 全 局 坐标 轴 的 45°" 翻 滨 、 俯 佩 和 偏 航 的 旋转 。 






































Mik 俯仰 偏 航 
图 2-9 全 局 翻 深 、 俯 仰 和 偏 航 的 旋转 


cBcY ”一 casyY 十 cysas8  sasy+cacysB 
ec ca cY sasBsy asar (2-55) 
一 SA cBsa ca ch 
假定 翻滚 角 、 俯 仰角 和 偏 航 角 ， 利 用 式 (2-55 我 们 能 够 综合 旋转 矩阵 。 当 一 个 旋转 拢 
阵 给 定 ， 我们 也 能 够 计算 其 等 效 的 翻 深 角 、 信 仰角 和 偏 航 角 。 假 设 x;,; 代表 翻滚 -俯仰 - 侦 航 
旋转 矩阵 中 第 i 行 第 ;7 列 的 元 素 ， 则 其 翻 深 角 为 












































a =arctan( 2) (2-56) 
r33 
俯仰 角 为 
B= arcsin (rs) (2-57) 
扁 航 角 为 
y=arctan{“) (2-58) 
rit 


iE: 假设 cosB40. 

例 2-9 OR MRSA. MA. Oi AA 

让 我 们 决定 所 求 取 的 翻滚 角 、 俯 仰角 、 偏 航 角 使 刚体 坐标 系 B 中 的 x 轴 和 平行 于 矢量 &， 
与 此 同时 y 轴 位 于 平面 (X，Y) A. 
































u =Î +2] 十 3 天 (2-59) 
因为 zx 轴 必 须 沿 着 矢量 w ， 因 此 有 
a Na wee (2-60) 
lu | ia 14 4 
因为 y 轴 位 于 平面 (X,Y) 内 ， 因 此 有 
cj=(f -e j) I+(J j) J= cosh 十 sin0 (2-61) 








轴 Si 和 67 必须 正 交 ， 因 此 有 
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1l/V14| fcosg 
[on] = (2-62) 
3/VI4 9 
0 一 一 26. 56° (2-63) 
可 以 利用 叉 积 求 取 嘱 为 
1/V14 | 0. 894 上- | 0. 358 | 
GR=S7X6j=|9/,/7g |X| —0. 447 0.717 (2-64) 
3/./1E 0 一 0. 597 
因此 ， 变 换 矩 阵 G@ 为 
T-i I+; Tek 1//14 0.894 0.358 
CQn=|F ei Jej .= 一 2/V 一 0.447 0.717 (2-65) 
K.i Keg Rek) av 还 0 一 0. 597 
现在 ， 从 与 全 局 坐标 系 G 有 共同 方向 的 刚体 坐标 系 B 移动 至 最 终 方向 ， 求 取 翻 滚 角 、 
俯仰 角 、 偏 航 角 是 有 可 能 的 。 
a=arctan (=) =arctan [二 =0 (2-66) 
B= —arcsin(rs1 ) =—aresin(3/,/14 )™—0. 93rad (2-67) 
y ~arctn( 2) =aretan{ EE J 1071ra (2-68) 
ru 1/f14 


2.4 绕 局 部 直角 坐标 轴 旋 转 


考虑 在 O 点 处 具有 空间 固定 点 的 一 个 刚体 B。 局 部 刚体 坐标 系 BCOzyz) 与 全 局 坐标 系 
G(OXYZ) 重合 ， 两 个 坐标 系 的 原点 均 在 固定 点 O 处 。 如 果 刚 体 如 图 2-10 所 示 那 样 绕 局 部 
坐标 系 的 轴 旋 转 o 角 ， 在 局 部 坐标 系 和 全 局 坐标 系 中 刚体 上 任何 点 的 坐标 通过 下 面 的 方 
程 相 关联 : 
































Br 一 4 人 -.pSr (2-69) 
矢量 sr AB 分 别 是 全 局 和 局 部 坐标 系 中 点 的 位 置 矢 量 : 
Sr 一 (X Y Z)! (2-70) 
Br=(x y z)! (2-71) 
Aco T = 轴 旋 转 矩 阵 ， 
cosp sing 0 
| cosp 0 (2-72) 
0 0 1 





类 似 地 ， 绕 y 轴 旋 转 0 角 ， 绕 xz 轴 旋 转 少 角 ， 可 以 分 别 用 y HE Fe FEE A yo Fle 轴 旋 
转 和 矩阵 Ai,y 描 述 
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cos 
Ayo=| 0 

sind 

1 
Ar. y= 





0 一 Sin0 

1 0 

0  cosd 

0 0 
cosy sing 
—sind cosy 





=X 





O 








图 2-10 


证 明 : 








X 


x 





点 卫 绕 局 部 坐标 系 中 x 轴 旋 转 前 后 的 位 置 矢 量 


用 矢量 r 表示 刚体 B 上 的 一 点 了 的 位 置 ， 该 点 的 起 始 位 置 在 P1 处 。 使 月 
R) 表示 沿 着 全 局 坐标 系 


(2-73) 


(2-74) 


日 单位 矢量 























(C> 了， 怀 ) 表 示 沿 着 局 部 坐标 系 B(COzyz) WAH. C, J, 
G(OXYZ) 的 坐标 轴 ， 最 初 位 置 "1 和 最 终 位 置 ~* 在 两 个 坐标 系 中 可 以 表示 为 
Bp 一 Zi 十 y17 zik 
cr =X Î +Y J +Z R 
Br, =x2t yop 二 zzk 
Cro =X:Î+Y:] +Z:R 


矢量 Br 和 Br ,是 矢量 r 在 刚体 坐标 系 BOrye) 中 的 初始 位 置 矢量 和 最 终 位 置 矢量 
HOr Ar, 是 矢量 r 在 全 局 坐标 系 G(COXYZ) 中 的 初始 位 置 矢量 和 最 终 位 置 矢量 。 
如 果 知 道 了 矢量 cm 的 分 量 和 内 积 〈 即 点 积 ) 的 定义 ， 那 么 利用 式 (2-78) 便 可 找到 矢 

















Ar, 的 分 量 ， 于 是 有 





1 i 
y2 |5 了 .了 了 
Zp kel k 
相对 刚体 坐标 系 Ozyz ， 绕 > 轴 旋 转 的 变 
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(2-75) 
(2-76) 
(2-77) 
(2-78) 
































kd YJ +i R ZoK 
站 YoJ i * ZK 
bd YJ + bd ZK 





oy 22k 
ef og *K |/ Yo 
ef ke K)\Ze 








El» 


(2-79) 
(2-80) 
(2-81) 


(2-82) 


换 矩 阵 4-.* 中 的 元 素 是 cr* WARK PR. Al 
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式 (2-82) 中 和 矩阵 的 元 素 为 


1。 了 一 cosp i .了 一 sinp i K= 
je Î=— sing jeJ = coso j e K=0 (2-83) 
k. Ì=0 ke j=0 “天 一 1 


合成 式 (2-82) 和 式 (2-83), Re A..s 乘 以 矢量 6r, 便 可 得 到 
EBr, 的 分 量 : 





x2 cosp sing 0)(X2 
y2 S cosp 0]| Ye (2-84) 
之 2 0 0 1/ (Z: 
其 简化 形式 为 
Br ,一 A-,p57， (2-85) 
式 中 ， 


cosp sing 0 


Az,g=|—sing cosp 0 (2-86) 
0 0 1 
式 (2-85) 表明 ， 绕 局 部 坐标 系 的 = 轴 旋 转 后 ， 局 部 坐标 系 中 的 位 置 矢量 等 于 x 轴 旋 转 
变换 矩阵 4-.* 乘 以 全 局 坐标 系 中 的 位 置 矢 量 。 因 此 ， 绕 = 轴 旋 转 后 ， 如 果 知 道 其 全 局 坐标 
的 话 ， 我 们 能 够 求 得 局 部 坐标 系 中 刚体 上 任何 点 的 坐标 。 
类 似 地 ， 绕 y 轴 旋 转 0 角 ， 以 及 绕 x 轴 旋 转 y 角 均 可 分 别 用 绕 y 轴 旋 转变 换 矩 阵 A,,。 


SE x FETS SE PR AEE A. THI 





















































cos? 0 一 Sin0 

Ayo=| 0 1 0 (2-87) 
sind 0 cosO 
1 0 

Aiy=|I0 cosy 可 (2-88) 
0 —sind cosy 





我 们 指出 ， 绕 局 部 坐标 系 轴 分 别 旋 转 p、 9、 y 角 的 第 1、 第 2 和 第 3 旋转 。 

Gi 2-10 局 部 旋转 ， 局 局 位 置 。 

如 果 一 个 局 部 坐标 系 Oxyz RE z 轴 旋 转 60"， 全 局 坐标 系 OXYZ 中 的 一 点 P 位 于 (4， 
党 


3，2) 处 ， 则 它 在 局 部 坐标 系 Ozyz 中 的 坐标 为 
4. 60 
is (2-89) 
2.0 
例 2-11 局 部 旋转 ， 全 局 位 置 。 


cos60” sin60” 0 
一 | —sin 60° cos60” 0 

如 果 一 个 局 部 坐标 系 Ozyz RH ez 轴 旋 转 60"， 全 局 坐标 系 OXYZ 中 的 一 点 P 位 于 4, 

3，2) 处 ， 则 它 在 全 局 坐标 系 OXYZ 中 的 位 置 矢量 为 




















X 

















0 0 











X cos 60° sin60” 0\T(4 —0. 60 
一 | 一 sin 60° cos60° 0 3 | 二 4. 96 (2-90) 
Z 0 0 1 2 Zed 
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例 2-12 连续 局 部 旋转 ， 全 局 位 置 。 





Ca 


图 2-11 例 2-12 中 的 机 械 臂 








如 图 2-11 所 示 的 机 械 手 臂 有 两 个 驱动 器 。 第 1 个 驱动 器 使 机 械 臂 绕 局 部 y 轴 旋 转 一 
90"， 然 后 第 2 个 驱动 器 使 机 械 臂 绕 局 部 x 轴 旋 转 90"。 如 果 末 端点 了 在 局 部 坐标 系 中 的 位 
BREN 











Brp 一 (9.5 一 10.1 10.1)7 (2-91) 
这 时 ， 全 局 坐标 系 中 的 位 置 矢量 为 











10. 1 
Gr, = (Ax.90°Ay,—90°) Brp=Ay, 00A ooBrp 一 4 了 90A! woBrp 一 区 | (2-92) 
9.5 


2.5 绕 局 部 直角 坐标 轴 连 续 旋 转 














刚体 B 上 一 点 也 在 全 局 坐标 系 中 的 位 置 用 矢量 > 表示 ， 绕 局 部 坐标 轴 旋 转 A Az, 
As, e, A, 后 ， 其 最 终 的 全 局 位 置 可 用 式 (2-93) 得 : 
Br =BAGGr (2-93) 





这 里 
BA —A,.…A3A:AI (2-94) 

其 中 ,BA6 被 称 为 局 部 旋转 矩阵 ， 它 可 将 全 局 坐标 变换 成 其 对 应 的 局 部 坐标 。 

绕 局 部 坐标 轴 旋 转 在 概念 上 是 很 有 趣 的 ， 因 为 在 一 个 序列 的 旋转 中 ， 每 一 次 旋转 都 是 绕 
着 局 部 坐标 系 中 的 坐标 轴 的 ， 最 后 一 次 旋转 将 局 部 位 置 移 至 其 新 的 全 局 位 置 。 

假设 知道 全 局 坐标 系 中 刚体 上 每 一 点 的 坐标 。 刚 体 和 它 的 局 部 坐标 系 绕 其 局 部 坐标 轴 旋 
转 ， 这 时 适当 的 局 部 旋转 矩阵 可 以 将 所 有 点 新 的 全 局 坐标 与 其 相应 的 局 部 坐标 相关 联 。 如 果 
引入 一 个 中 间 固 定 坐 标 系 ， 该 固定 坐标 系 与 刚体 坐标 系 的 新 位 置 相 重 合 ， 则 可 以 使 刚体 绕 局 
部 坐标 轴 进 行 第 2 次 旋转 。 现 在 男 一 个 适当 的 局 部 旋转 变换 和 矩阵 将 中 间 固 定 坐 标 系 中 的 坐标 
与 相应 的 局 部 坐标 相关 联 。 因 此 ， 所 有 点 的 最 终 全 局 坐标 必须 首先 被 转换 到 中 间 固 定 坐标 系 
之 中 ， 接 着 被 转换 到 最 初 的 全 局 坐标 轴 。 

Gi 2-13 连续 局 部 旋转 ， 局 部 位 置 。 
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一 局 部 坐标 系 BCOzyz) 初始 时 与 全 局 坐标 系 GCOXYZ) 相 重 合 ， 绕 z 轴 旋 转 30"， 接 
AGE x 轴 旋 转 30"， 最 后 绕 y 轴 旋 转 30"。 位 于 X= 二 5、 Y=30, ZZ 二 10 的 一 点 P, CWA 
部 坐标 (x y zx)T 二 Ay.,yAi.oh4:.o(5 30 10)T。 其 局 部 旋转 变换 矩阵 为 

0. 63 0.65 —0.43 
BAG =A, 50°Ax,30°A 2,30° =| —0. 43 0. 75 0. 50 (2-95) 
0.65 一 0.125 0.75 





在 局 部 坐标 系 中 点 P 的 坐标 为 


x 0. 63 0. 65 —0. 43) 5 18. 35 
y | =| 一 0. 43 0. 75 0. 50 || 30 |=] 25. 35 (2-96) 
z 0.65 —0.125 0.75) \10 7.0 


例 2-14 连续 局 部 旋转 。 
一 刚体 点 已 Cos ys z) RRIF Aor Arons A ,,y 后 ， 其 旋转 变换 矩阵 为 
cpcy—sOspsy cysptepsOsy —cOsp 
BA, =AyyAxr0Az.o = — cso côco SO (2-97) 
cosy 十 SOcWsPp sesp—cesbcep chey 
例 2-15 12 个 独立 的 3 次 局 部 旋转 。 
欧 拉 定理 : 任何 两 个 具有 共同 原点 且 独 立 的 正 交 坐标 系 通过 序列 地 绕 局 部 坐标 轴 的 3 次 
旋转 能 够 相关 ， 在 此 两 个 连续 旋转 不 能 绕 同一 轴 进 行 。 总 之 ， 绕 局 部 坐标 轴 的 3 次 旋转 有 
12 种 不 同 的 独立 合成 方式 ， 分 别 为 

















Rb 
SRR 
6 6 8 


n 


aS 
> 
RS 
> 
n 

$ 


(2-98) 


二 六 
> & 
> 
“8 


n 


>> >> PDD DD DD 
< = ar a = y . < . 
> > 
> > 
s S 


$ 
> 
> 

= 


Az yA y0Axz,o 


局 部 坐标 轴 的 3 次 旋转 的 扩展 形式 见 附 表 B。 





2.6 欧 拉 角 


绕 全 局 坐标 系 的 Z 轴 的 旋转 被 称 为 旋 进 ( 即 进 动 ，precession)， 绕 局 部 坐标 系 的 x 轴 
的 旋转 被 称 为 章 动 (nutation)， 绕 局 部 坐标 系 x 轴 的 旋转 被 称 为 自转 (spin)。 旋 进 章 动 自 
转角 (precession-nutation-spin rotation angles) 也 称 为 欧 拉 角 。 欧 拉 角 旋转 矩阵 在 刚体 运动 
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图 2-12 第 1 KKA AA 


学 中 有 许多 应 用 。 为 了 找到 从 全 局 坐标 系 G(OXYZ) 到 最 终 的 局 部 坐标 系 B(Ozyz) 的 欧 
拉 角 旋转 和 矩阵， 使 用 如 图 2-12 所 示 刚 体 坐 标 系 B Ory z), BARRER 1 次 旋转 之 前 与 
全 局 坐标 系 G(OXYZ) 重合 。 假 设 第 1 次 绕 z' 轴 旋转 wo 角 。 因 为 Z 轴 与 >' 轴 是 重合 的 ， 因 














此 有 
Br =P Aor (2-99) 
cosp sing 0 
BAG=Az.e= —sing coso 0 (2-100) 
0 0 1 








接着 考虑 将 坐标 系 B (Ory z) 作为 一 个 新 的 固定 全 局 坐标 系 ， 并 且 引 入 一 个 新 的 坐 
标 系 B (Ox”y”z”)。 在 第 2 次 旋转 之 前 ， 两 个 坐标 系 重 合 。 这 时 ， 绕 x" 轴 旋转 0 角 如 图 2-13 
FR. BERA B'(Oz'y'z') 与 坐标 系 B (Ox”y”z”") 之 间 的 变换 为 








B'r =P Aap (2-101) 
1 0 0 
BTA Bt =A,.0=|0 cos@ sing (2-102) 
0 一 Sin0 cos 

















图 2-13 第 2 欧 拉 角 











最 后 ， 我 们 考虑 将 坐标 系 BOr y: 作为 一 个 新 的 固定 全 局 坐标 系 ， 并 且 考 虑 在 第 3 
次 旋转 之 前 最 终 的 刚体 坐标 系 B(COzyz) 与 坐标 系 B (Ory: 重合 。 现 在 ， 我 们 绕 x Mie 
转 y 角 ， 如 图 2-14 所 示 。 坐 标 系 B'(Ox" yz") 与 坐标 系 ee 之 间 的 变换 为 
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Br =BA y?r (2-103) 
cos sing 0 
BA y =A: „=| sing cosy 0 (2-104) 
0 0 1 
通过 合成 旋转 规则 ， 从 坐标 系 GOOXYZ) 到 坐标 系 B(Ozyz) 的 变换 为 
Br = Aer (2-105) 
这 里 
cocy 一 cOSPSW cWSPD 十 cgcpSW SOSW 
BA 5A, yA 0A z. p =| 一 cpSW 一 cgcWsp sosy tchcpey sOcy (2-106) 
SOSD — cosh c0 
因而 


cpcy—cOspsy cpsy—cOcysp sso 
GQ =BA G =BAL H(A. yArAz.g)' =|chsot+cbcosp —spsp+chcpey 一 cpsb 
sOsy sOcy cO 

(2-107) 








假设 给 定 旋 进 角 pg、 章 动 角 0 和 自转 角 y， 可 以 利用 
式 (2-106) 计算 综合 旋转 矩阵 。 当 一 个 旋转 矩阵 给 定时 ， 
也 能 计算 等 效 的 旋 进 角 、 章 动 角 和 自转 角 。 

如 果 六 表示 旋 进 章 动 自转 旋转 矩阵 式 (2-106) 中 第 
i 行 和 第 j 列 的 元 素 ， 这 时 有 














0 一 arccos(733) (2-108) 
p= —aretan(“) (2-109) 
32 
p= arctan 2>] (2-110) 
r23 





假设 sindA0, 
例 2-16 “ 欧 拉 角 旋转 变换 矩阵 。 
对 于 p= 二 79.15"、 0 二 41. 41°, y= 一 40.7° 的 欧 拉 或 
者 旋 进 、 章 动 、 自 转 旋转 变换 矩阵 可 通过 取代 式 (2-106) 中 的 参数 p、 0、 pR: 
0. 63 0.65 —0. 43 
BAG =Az,—40.7Ax,41.4Az,79. 15° =| —0. 43 0.75 0. 50 eth 
0.65 一 0.125 0.75 























例 2-17 局 部 旋转 变换 矩阵 的 欧 拉 角 。 
绕 > 轴 旋 转 30°, PEASE x 轴 旋 转 30”"， 最 后 绕 y 轴 旋 转 30 之 后 ， 局 部 旋转 变换 拢 





0.63 0.65 一 0.43 
BA .一 Asa04Avso4A-sao 一 | 一 0.43 0.75 0. 50 (2-112) 
0.65 一 0.125 0. 75 
因此 ， 在 X=5、Y=30、Z=10 处 样本 点 的 局 部 坐标 为 
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ya 0. 63 0. 65 —0. 43) (5 18. 35 
y | 三 | 一 0. 43 0.75 0.50 | |30 |=]|25. 35 (2-113) 
z 0.65 —0.125 0.75 ) \10 7.0 


ABO EE. BRON. H PENERE AE EY RR i FA 
8=arccos(0. 75) =41. 41° 


0. 65 
=— an| ———— ]=79. 15° 
p arctan( | 
一 0. 43 
一 a == £ 
y arctan{ 0.50 ) 40 (2-114) 


因此 ， 当 p=79.15°, 0=41. 41°, p=—40. 7°, Ayer Axz,30°Az.30° =AzyAreAz.go 
teint. WIA ze. xy z BAD BIER GO =79.15°, 0=41.41°, P= —40. OMA z, xy 
y 轴 分 别 旋转 yg 二 30"、0 二 30”"、y 二 30”"， 附 着 于 局 部 坐标 系 上 的 刚体 能 够 移动 到 最 终 位 置 。 

例 2-18 两 刚体 的 相对 旋转 变换 矩阵 。 

相对 于 全 局 坐标 系 ， 考 虑 具有 由 欧 拉 角 gp 一 30"、0 王 一 45"、% 一 60" 所 形成 的 定向 变换 矩 
MB Ag 的 刚体 B MAA @=10°, 0=25°. P=—15° 的 另 一 刚体 B: 。 为 了 求 得 从 第 2 个 刚 
体 坐 标 系 Bo 变换 到 第 1 个 刚体 坐标 系 Bi 的 相对 旋转 矩阵 za A, ， 首 先 需 要 找到 每 个 旋转 变 
换 矩 阵 。 














0.127 0.78 —0.612 
Bi AG SA: 60 Ax, —45° Åz, 30 =| 0.927 一 0.127 一 0. 354 (2-115) 
—0. 354 0. 612 0. 707 
0.992  —0. 0633 一 0. 109 
BAG=Az,10°Az,25°Az,—15° =| 0.103 0. 907 0. 408 (2-116) 
0.0734 一 0. 416 0. 906 
期 望 的 旋转 变换 矩阵 2 A p, 可 以 通过 式 (2-117) 求 得 : 

BAp,—=MAG Ap, (2-117) 








它 等 于 
0.992 0.103 0. 0734 
Bi Ab 一 BAGPA4LU 一 | 一 0.0633 0.907 一 0.416 (2-118) 
一 0. 109 0.408 0. 906 
例 2-19 关于 小 角度 的 欧 拉 角 旋转 变换 矩阵 。 
对 于 很 小 的 欧 拉 角 o, 0, pe ALIER AE IEB AG =A yA roA p HWA 











l y 0 
BAc=|—y 1 9 (2-119) 
0 一 入 1 
这 里 
y=ot9 (2-120) 


因此 ， 在 小 角度 旋转 的 情况 下 ， 角 度 pg Ald 是 无 法 区 别 的 。 
例 2-20 较 小 的 第 2 欧 拉 角 。 
如 果 OO, CEM RK Hit Wie Fe AE MEP A G =A ev yAxoAz,o AWN 
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因此 ， 角 度 p 和 中 是 无 法 区 别 的 ， 甚 至 即使 pp 和 由 的 值 是 已 知 的 。 因 此 ， 当 0=0 时 ， 
在 式 (2-106) 的 旋转 变换 矩阵 中 的 欧 拉 角 组 并 不 是 独立 的 。 

例 2-21 刚体 运动 中 欧 拉 角 的 应 用 。 
用 于 描述 具有 固定 点 刚体 配置 的 zzz 欧 拉 角 是 很 好 的 参数 。 作 为 一 个 例子 ， 展 示 顶 部 
配置 的 欧 拉 角 如 图 2-15 所 示 。 






























































a 

















图 2-15 欧 拉 角 在 描述 顶部 配置 方向 的 应 





BAG SA: Åz. 4-p 王 | 一 cpsW 一 cWsSP —sesptcocd 0 
0 0 1 
| cos(p ty)  sin(g +P) | 





coc — sosy cWsp 十 cpSsy | 





二 | 一 SinCP 十 %) cosCP 十 %) 0 (2-121) 
0 0 1 
KG 2-22 ”以 欧 拉 角速度 为 单位 的 角 速 
ERE. 
通过 定义 单位 矢量 ev ĉo, êy 引入 欧 拉 
局 部 坐标 系 E(Co、cv 、6n 、6y)， 如 网 2-16 所 
示 。 昌 然 欧 拉 局 部 坐标 系 EO, êg, ĉon êg) 
不 必 是 正 交 的 ,但 是 在 刚体 运动 分 析 中 它 是 
非常 有 用 的 。 
相对 于 全 局 坐标 系 GCOXYZ), 刚体 坐 
标 系 B(Ozyz) 的 角速度 矢量 ows 可 以 被 写 在 
欧 拉 局 部 坐标 系 中 ， 作 为 3 个 欧 拉 角 速率 
矢量 之 和 。 
Ew p=Geet+Oegtwey, (2-122) 


式 中 ， 欧 拉 角 速度 p 、6 My 称 为 欧 拉 频 率 。 
为 了 求 得 刚体 坐标 系 中 的 角速度 co@ 8， 必 须 在 刚体 坐标 系 中 表示 单位 矢量 êp, Cay bys 
































图 2-16 欧 拉 局 部 坐标 系 ev eo. ey 
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如 图 2-16 所 示 。 单 位 矢量 cr 一 [0 0 ] =R 位 于 全 局 坐标 系 中 ，3 次 旋转 之 后 它 能 被 转 
换 至 刚体 坐标 系 之 中 。 
Be, =BAGK =A. yA 1 0A z, pK = (2-123) 


sind coso 





sind os 


COSO 
单位 矢量 ég=(1 0 o =i EPER Ory’ 内， 需要 进行 两 次 旋转 A, 和 A.,y 
后 ， 便 可 被 转换 至 刚体 坐标 系 中 。 





cosy 
ee (2-124) 
0 
单位 矢量 ev 已 经 位 于 刚体 坐标 系 6y 一 (0 0 DT= 中。 因此 ,ec ws 在 刚体 坐标 系 
中 可 表示 为 

Sin0siny cosy 0 

BoB =9 se +6 E 0 

cos 0 1 











(9 sindsin +0 cosy)i 十 (9 singcosy —0 sing)j 十 (9 cosd+y yk (2-125) 
因此 ， 在 刚体 坐标 系 Ozyz Poor 的 分 量 通过 下 列 关 系 与 欧 拉 角 坐 标 系 Og0y 相关 联 : 














op 一 BA4EE6wB (2-126) 
Wx sind sind cosp 0 p 
wy |=| sinĝcosy 一 sinw 0|1|0 (2-127) 
wz cos? 0 1 本 
y 


这 时 ， 通 过 欧 拉 旋转 矩阵 式 (2-106) 的 道 变换 eco 可 以 在 全 局 坐标 系 中 表示 为 


9 sind sing +ô cosy 





G 二 LA coe 
GOB 一 46 G@n="Ag | @ sindcosd—@ sing 





p cosi +y 
= (6 cose +y singsinp )I + (6 sing—y singcosp )j + (9 +y cos0)K (2-128) 
因此 ， 在 全 局 坐标 系 OXYZ 中 cws 的 分 量 通过 下 列 关系 与 欧 拉 角 坐标 系 Opby 相 














关联 : 
Eaop 一 GOFEawB (2-129) 
wx 0 coso sind sing 9 
wy |=|0 sing 一 cospsin0 ||0 (2-130) 
WZ 1 0 cos? 





ġ 

WB 2-23 基于 备 卡 儿 角 速度 矢量 的 欧 拉 角速度 。 

Rit Gon 指 关 于 全 局 坐标 系 G 在 刚体 坐标 系 B 中 刚体 B 的 角速度 。 矢 
关系 与 欧 拉 频率 相关 联 : 











ten 
è 
E 
ef 
=| 
a 
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6@n="Arc@s (2-131) 
Wx sindsing cosy 0119 
E@B=|w, |=|singcosy —singd 01I6 (2-132) 
COSO 0 Ly 
e ġ 
系数 和 矩阵 并 不 是 一 个 正 交 矩阵 ， 即 
BARABA p. (2-133) 
sinfsiny sinĝcosy cosð 
B4 一 cosw — sing 0 (2-134) 
0 0 1 
í sing cosg 0 
BA —sinĝsi 7 
Ay sind sind cosw sindsing 0 (2-135) 
—cosdsing 一 cosOcosW 1 





这 是 因为 欧 拉 角 坐标 系 Og0y 不 是 一 个 正 交 坐标 系 。 因 为 同样 的 原因 ， 跟 欧 拉 角 频率 相 
关 的 系数 矩阵 和 角速度 Eoa 的 分 量 也 不 是 一 个 正 交 和 矩 阵 。 











Cos = QELOSB (2-136) 
wx 0 coso sind sing 9 
LQ@B—=|wy |=|0 sing —cosgsiné || 6 (2-137) 
WZ 1 0 cosd j 





因此 ， 基 于 局 部 和 全 局 的 角速度 矢量 cwa OD HE EY DC Fi Ba ST Da a at FR CA PEE — 
求 得 : 














op 一 24E COR (2-138) 
9 i sing cosy 0) (wx 
0 | 一 sin0cosm  —sindsingd O||wy (2-139) 
é —cos@sing 一 cosOcosW 1)\w- 

Gor = Qr Cos (2-140) 
9 —cos@sing cosOcosP 1)\(wx 

Pr l . si 

0 | me sinfsing 0]| wy (2-141) 
y sing — coso O)lwz 


利用 式 (2-138) 和 式 (2-140)， 可 以 验证 ， 变换 矩阵 BAc 王 84FSOFf1 与 式 (2-106) Kk 
拉 变 换 所 得 的 矩阵 相同 。 
这 样 一 来 ， 角 速度 矢量 可 以 表述 为 





Wy wx 9 
cmp—(i j kjloy|=( f K)loy|=(K és £)l6 (2-142) 
CU = WZ ġ 
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交 例 2-24 角速度 分 量 的 可 积 (分 ) 性 。 
对 于 函数 f= 二 f(x,y) 的 任意 一 个 总 的 微分 : 






































af, af 
df=fidr+fedy =5-dx +>—-dy (2-143) 
OX dy 
其 可 积分 的 条 件 为 
Ifi Afe 
o (2-144) 
沿 着 刚体 坐标 轴 x、y、xz 的 角速度 分 量 wx wy, we 并 不 能 通过 积分 获得 相关 的 角 
因为 : 
w+ dt =sindsingdg 十 cosWdO0 (2-145) 
d(sin@sind)) ,9cosy 
20 Jo (2-146) 
然而 ， 欧 拉 角 速度 能 够 满足 式 (2-144) 的 积分 条 件 。 从 式 (2-139) 可 得 
_ sind) cosy 
P= nd) tg (wy dt) (2-147) 
dO 二 cosy (w+ dt) — sing (wy dt) (2-148) 
TE OE Ty Le ae ee a (2-149) 
sind sind sind 
例如 ， 第 2 个 方程 表明 : 
ag a0 
cosg Iodo’ se od (2-150) 
因此 ， 有 
cos)  ., ,_ 9% sinycoslcosy 
Mag. Y ot aad kato? 
a(—~sing) — dap _ sindcosécosp g 
asd) T Nast) sind eee 





同 理 ， 我 们 可 以 验证 : do 和 dy 也 是 可 积分 的 。 

*% Gi 2-25 卡尔 丹 角 (Cardan angles) 和 角速度 。 

当 0 二 0 时 ， 欧 拉 角 系统 是 奇异 的 ， 因 而 pg 和 y 变 成 共 面 角 并 且 难 以 辨别 。 在 附录 BE 
AH 12 种 角 系 统 内 ， 每 种 角 系 统 的 名 称 、 特 征 、 优 缺点 中 绕 着 3 个 不 同 轴 的 旋转 变换 ， 例 
WPA GSA: yAy Axo RAMA KARST AA A AA (Bryant angles)。 当 0 二 0 F, FRIKA 
角 系 统 是 非 奇 异 的 ， 并 且 在 中 心力 场 的 机 电 一 体 化 和 卫星 姿态 分 析 中 有 一 些 应 用 。 

















cocy cpsytsOcysp sgsy— cosco 
BAG=|—cOsy copcy—sOspsy cysptepsOsy (2-153) 
SO 一 cOSp cOcp 


刚体 角速度 @ 可 以 根据 坐标 系 B (Oryz) 中 沿 着 轴 的 分 量 进 行 表 述 ， 或 者 根 据 非 正 交 
卡尔 丹 坐 标 系 中 沿 着 轴 的 卡尔 丹 角 频率 〈 速 度 ) 进行 表述 。 依 据 卡 尔 丹 角 频 率 ， 角 速度 可 表 
示 为 
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1 0 0 
sorte} rb (2-154) 
0 0 1 

















因此 ， 有 
Wx cos@cosp siny 0 9 
wy |=|—cosésing cosy 0llg (2-155) 
We sind 0 1)| > 
y 
` cosy sind 
r cos cos d 
0 sing cosy o | ”> Sete) 
y —tanĝsinð tandsing 1) \°* 
对 于 较 小 卡尔 丹 角 的 情况 ， 有 
1 g 0 
BAg=|—¢ 1 (2-157) 
0 —ọ 1 
Wr 1 p 0 ? 
wyl/=|—¢ 1 06 (2-158) 
Ws 0 0 1 。 
y 


2.7 局 部 翻滚 角 、 俯 仰角 、 偏 航 角 


绕 局 部 坐标 系 中 x 轴 的 旋转 被 称 为 翻 深 ; 绕 局 部 坐标 系 中 > 轴 的 旋转 被 称 为 俯仰 ; 
绕 局 部 坐标 系 中 z 轴 的 旋转 被 称 为 偏 航 。 局 部 翻 演 角 、 信 仰角 、 偏 航 角 如 图 2-17 
所 示 。 

局 部 翻滚 -俯仰 - 偏 航 旋转 矩阵 为 

BAG=Az,yAy, Azo 

ccd cosy + sO cdsq rs) wit 一 | 一 6 























N 





二 | 一 cOSW cocd—sOsgsp cpse+cesbsp 
SO 一 cOSsp cOcp 
(2-159) 
虽然 我 们 利用 o. 0 及 y 表示 翻滚 角 、 
信仰 角 、 偏 航 角 与 欧 拉 角 ， 但 这 里 需要 注 g 
意 两 者 的 区 别 。 ae 总 iim 
KB 2-26 ”角速度 和 局 部 翻滚 角 速 翻滚 -0 
率 、 信 仰角 速率 、 偏 航 角 速率 。 

















图 2-17 局 部 翻 深 角 、 俯 仰角 、 偏 航 角 
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利用 翻滚 角 速 率 、 俯 仰角 速率 、 偏 航 角 速率 ,刚体 B 相对 于 全 局 坐标 系 的 角 速 率 为 
Gop 一 ou itwyjtwo.k=@egtO eo ty ey (2-160) 
当局 部 翻滚 单位 矢量 ev 、 俯 仰 单位 矢量 co 被 转换 至 刚体 坐标 系 中 时 ， 可 以 求 得 刚体 
Poon 的 分 量 与 翻滚 分 量 、 俯 仰 分量 及 偏 航 分 量 之 间 的 关系 。 翻 滚 单位 矢量 ev 王 
(1 0 oj 在 旋转 0 角 后 被 转换 至 刚体 坐标 系 中 ， 然 后 再 旋转 少 角 ， 则 有 























1 cosOcosy 
Bey—=A:.yAy.010|=|—cosOsing (2-161) 
0 sind 
俯仰 单位 矢量 eo= (0 1 0)7 在 旋转 y 角 后 被 转换 至 刚体 坐标 系 中 ， 则 有 
0 sind 
eb =| cosy (2-162) 
0 0 























WARE ê= (0 0 1)T 已 经 是 沿 着 局 部 坐标 轴 的 x 轴 方 向 ， 因 此 在 刚体 坐标 
系 Ozyz 中 c@s 的 表达 式 为 


























Wx cos0 cosg sing 0 cosdcosf siny 0 9 
Bop=|w, |=@ | —cosdsing | 十 bg | cosp|+w]0}=|—cosdsing cosy 0 0 
We sind sind 1 sind 0 1)| > 
y 
(2-163) 
因此 ,依据 局 部 翻滚 角 速 率 、 俯 仰角 速率 、 偶 航 角 速率 ， 全 局 坐标 系 OXYZ 中 的 
GOB 为 
wx W > 0 sing+9 cosl cosg 9 +y sind 
G@z=|wy |=BAG'|wy |="AG'|6 cos —@ cos@sinw | 一 6 cose— y cos0 sing 
pie oe y +@ sind 6 sing +y cosl coso 
1 0 sind 9 
一 cose 一 cosgcosp || @ (2-164) 
0 sing  cosdsing 


2.8 局 部 轴 对 全 局 轴 旋 转 


全 局 旋转 矩阵 5O@a 等 于 局 部 旋转 和 失 阵 ?4c 的 逆 和 矩阵 ， 反 之 亦 然 。 








GOp=BAc!, BAG ="Q5! (2-165) 
这 里 

GQR =A] ŻA; A3 TI A, (2-166) 

BAc=Q; Qz 050， (2-167) 





BEJE He 4 Sk ERI A AR E T A a He Pe BE A AR. 
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证 明 : 
考虑 一 系列 的 全 局 旋转 和 将 一 位 置 矢量 3r 转换 至 Sr 的 合成 全 局 旋转 矩阵 5Ob 。 
Gr =SQpPr (2-168) 
利用 局 部 旋转 矩阵 BA4c ， 全 局 位 置 矢 量 sr 也 能 被 转换 至 By。 
Br =BAGër (2-169) 
综合 式 (2-168) 和 式 (2-169), AI: 
Gr 一 GOBB4AcGSr (2-170) 
Br =BAGCQpBr (2-171) 
因此 ， 有 
GQOpBAc=AcSOp=I (2-172) 
因此 ， 全 局 旋转 矩阵 和 局 部 旋转 和 矩阵 互 逆 ， 即 
GOp=BAG!;SOQp!=BAc (2-173) 
假设 iQp 二 0,…0302Q1 MPAG SA, AAA MWA 
GQOp=BAc!'=AT!A; A; VeA, ! (2-174) 
BAcG—=°Q8 =Q1 Q7 Q3 0, (2-175) 
式 (2-172) 变 为 
On QQ An AA SAn AA Qn 0201 =1 (2-176) 
因此 ， 有 
Q, QQ =A A7 A7 A7! (2-177a) 
An A241 =01 1037 Q; eQ, (2-177b) 
或 者 
Q110;1031*0, 10,..…0201=I (2-178) 
A'A A; UA, Ant AASI (2-179) 





KE, EE Jay A pR Ah FE SE NE FE I RR Ee SE Ja aS A os i F D SE E 4 A E fA BE DE He KY 
效果 。 

例 2-27 ”全 局 位 置 和 旋转 矩阵 的 自 右 乘 。 

点 卫 旋转 之 后 的 局 部 位 置 为 3r =(1 2 3)7T。 如 果 将 Sr 转换 至 Br 的 变换 矩阵 ; 











cosp sing 0 cos 30° sin 30° 0 
BA..g=|—sing cosp 0| 王 | 一 sinp 30° cos 30° 0 (2-180) 
0 0 1 0 0 1 


XH, RIDE A ARPA: o 和 局 部 位 置 矢量 3r ?， 可 求 得 全 局 位 置 矢量 Sr : 
cos30” sin30” 0 
GrT=BrTBA,,s=(1 2 3)|—sin30° cos30° 0|=(—0.13 2.23 3.0) 








0 0 1 
(2-181) 
HPA. "与 ar WAAR, 有 
cos30” sin30” OA 一 0. 13 
Gr =BA 7 Br 一 | 一 sin30” cos30° 0||2|=| 2.23 (2-182) 
0 0 1 八 3 3.0 
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2.9 ”一般 变换 





考虑 具有 两 个 坐标 系 的 一 般 情况 ， 这 两 个 坐标 系 G(OXYZ) 和 BCOzyz) 具有 共同 的 
原点 O， 用 来 表达 矢量 r 的 分 量 。 总 是 有 一 个 变换 和 矩阵 5 Rs 将 r 的 分 量 从 参考 坐标 系 B 
(Oxyz) 转换 至 参考 坐标 系 G(OXYZ ) 。 














cr 一 “RBBr (2-183) 
除 此 之 外 ， 利 用 3BRG 可 进行 逆转 换 ar 一 GR Or, BI 
Br 一 BRCGr (2-184) 
这 里 
[SRg |= |3RG | =1 (2-185) 
BRo=°R,!=SR} (2-186) 
iE AA : 
坐标 系 G(OXYZ) 沿 坐标 系 BCOzyz) 轴 的 单位 矢量 分 解 ; 
T=U+Ditd-+jp j++ kk (2-187) 
J= Dit -IIH + bk (2-188) 
K=(K *Dit(K j) HK +k (2-189) 
引入 将 局 部 坐标 系 变换 至 全 局 坐标 系 的 变换 矩阵 SRB : 
I Tei TY i 
jsj fey Fek |l7 |=SRalz (2-190) 
ix 


T-i I+j Tek cos(I,i) cos(Î,j) cos(i,k) 
GRa=|jJei J “jj J ek |=lcos(J.i)  cos(f ,7 ) cos(J ,k) (2-191) 
R-i K+j Rek cos(K si) cos(K,j) cos(K .k) 
变换 矩阵 SRaB 的 每 列 是 局 部 坐标 系 BCOryz) 中 的 单位 矢量 在 全 局 坐标 系 G(OXYZ) 
中 的 分 量 ， 即 

















GRp=(Gi Sj Gk]=(ro ro Fus) (2-192) 
类 似 地 ， 变 换 矩 阵 5Ra 的 每 行 是 局 部 坐标 系 B (Oxyz) 中 的 单位 矢量 在 全 局 坐标 系 
G(OXYZ) 中 的 分 量 ， 即 











BÎT TH, 
Gp lagT lhe, (2-193) 
BKT ra 





变换 矩阵 SRB 中 的 每 个 元 素 是 在 局 部 坐标 系 B(COzyz) 中 沿 着 G(OXYZ) 轴 的 方向 余 
弦 。 这 组 中 的 9 个 方向 余 改 可 完全 确定 局 部 坐标 系 BCOxyz) 在 全 局 坐标 系 GCOXYZ) 中 的 
定位 ， 并 且 能 将 任何 点 的 局 部 坐标 Co, ye x) 转换 到 其 对 应 的 全 局 坐标 (X,Y，2Z)。 
或 者 ， 使 用 单位 矢量 分 解法 求 取 转换 矩阵 SRG ， 则 有 
Br =BRoSr 一 SGREIGr (2-194) 
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iel iej i¢K cos(i,1) cos, Ĵĵ) cosG,K) 
BRo=|jel jef j *Kl=lcosG.1) cosg.) cos(j,K) (2-195) 
hel keJ keK cos(Å,Î) cos(k.J) cos(k,K) 
De EL. A ZE HRE PE KS a St PE ip RIS Pe IE : 
GR 一 GRE (2-196) 
HA (2-196) 特性 的 矩阵 被 称 为 正 交 和 抢 阵 。 和 抢 阵 民 的 正 交 性 来 自 这 样 的 事实 : 它 能 





将 一 个 正 交 坐标 系 映射 到 另 一 个 正 交 坐标 系 。 











变换 矩阵 R 只 有 3 个 独立 的 元 素 。 利 用 式 (2-196) 的 正 交 和 条件， 可 以 求 得 矩阵 R 中 元 


素 的 约束 方程 为 
GRD 1 e GR 
rit 三 12 Tis rii r21 
7 21 r22 r 23 r12 r22 
r3i Vr 32 133 r13 r23 


因此 ， 和 矩阵 $Rp 中 任何 两 个 不 同行 的 点 积 等 于 零 ， 





行 的 点 积 等 于 1 。 





x 
x 
Fo 





x 
x 


N 











WN wwe Hd 
wd wwe Hd 


x 
Nx 
N 





T11721 T12722 





7T117 31 T12732 











T31721 7 327 22 


TE 

Bs =I (2-197) 
r31 1 0 0 
r32 |=|0 1 0 (2-198) 
T33 0 0 1 





J HESR, 中 具有 相同 行 的 任何 











Îi =1 

23 = 1 

s 5l (2-199) 
Pist 33 =0 

rits =0 

r23r33 =0 


对 于 和 矩阵 "Ra 的 列 ， 以 及 BRG 的 行 和 列 ， 这 些 关 系 也 适应 。 正 交 性 条 件 总 结 如 下 : 


3 
m 7 加 oe 
YH; ° YH; =H," H; = WM rira =ð; (Gok =1,2,3) 


i=1 


(2-200) 





KE, x; eR RP TB | 列 的 元 素 ; 5; 是 克 罗 内 克 o 图 数 (Kronecker’s del- 


ta), 一 个 二 元 函数 。 


如 果 7 =k M Sj = 15 HR j Ak Ml dix 一 0 


IN (2-200) 提供 了 9 个 方向 余弦 必须 满足 的 


(2-201) 
6 个 独立 关系 。 仅 仅 有 3 个 独立 方向 余弦 。 











ERER 中 的 独立 元 素 显 而 易 见 并 不 是 相同 的 行 或 列 ， 或 者 任何 对 角 线 。 





依据 式 〈2-197) ， 变 换 和 矩阵 的 行列 式 等 于 1, 





|[SRg|=1 (2-202) 
注意 
|SRB。SRE| 一 |SRe|l |ORZ|=|FRB| | Re|=|°Re|’=1 (2-203) 
Al FA Ze PETRA SE MEO RE PA Retr,» re, 和 rm, ， 我 们 可 以 知道 : 
I@Rel=rh, * Ga Xrps) (2-204) 
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由 于 坐标 系 是 符合 右手 定 则 的 ， 因 此 有 


rH, XrHs 一 ”Fi， 即 


[SRs|=rh, orn, =le 
例 2-28 变换 矩阵 中 的 元 素 。 


依据 相对 于 全 局 坐标 系 G(OXYZ) 或 者 局 
部 坐标 系 B(Ozyz) 的 分 量 ， 可 以 表述 点 了 的 
位 置 矢量 r 。 刚 体 和 全 局 坐标 系 如 图 2-18 所 示 。 











N 








如 果 Sr =1001—50f +150K. E$ h Æ RI RAB 
标 系 B(COzyz) 中 的 分 量 ， 必 须 首 先 求 得 适当 
的 旋转 和 矩阵 BRc 。 

FE REP RG 的 行 元 素 是 局 部 坐标 系 BCOxyz) 
在 全 局 坐标 系 G(OXYZ) 中 坐标 轴 的 方向 余 
a x 轴 位 于 平面 XZ 内 与 X 轴 夹 角 40"，y 轴 




















y 
图 2-18 {j 2-28 的 刚体 Ca 











坐标 系 和 全 局 坐标 系 
All Y 轴 之 间 的 夹 角 为 60"。 因 此 ， 有 
ie] iej iK cos 40° 0 cos 40° 0. 766 0 
BRo=|j°l jef geKl=| 72 cos60° je eK |=|7eI 0.5 
kel keJ keK keT kj) kK kel keJ 
利用 BRGSRB 一 SRG .SRG 一 I 
0.766 0 0.643110.766 rai rai 1 0 0 
r21 0.5 r 23 0 0.5 r3 |=|0 1 0 
r31 r32 r33 0.643 r23 r33 0 0 1 
可 以 获得 一 组 方程 ， 以 求 得 未 知 元 素 : 
0. 766r21 F0. 643r23 =0 
0. 766r31 +0. 643r33 =0 
roi tris +0. 25=1 
r21731 十 0. 5r32 十 723733 一 0 
ene a Gia 一 1 
求解 这 些 方程 ， 可 得 下 面 的 转换 矩阵 : 
0.766 0 0. 643 
BRo=| 0.557 0.5 —0. 663 
—0.322 0.866 0.383 
因此 ， 可 以 求 得 矢量 ar 的 分 量 。 
0.766 0 0. 643 100 173. 05 
Br = "her =| 0.557 0.5 ， 一 0.663 || 一 50 |=| 一 68.75 
—0. 322 0.866 0.383 150 —18. 05 
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例 2-29 利用 局 部 矢量 ar 和 变换 矩阵 8Rc 求 其 全 局 矢量 Sr 。 








0. 643 
jek 
(2-205) 


(2-206) 


(2-207) 


(2-208) 


(2-209) 


在 全 局 坐标 系 G(OXYZ) 或 者 局 部 坐标 系 B(Ozyz) 中 ， 可 以 描述 点 已 的 位 置 矢 量 r 。 
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如 果 Br 二 1001 一 507 十 150&， 以 及 下 面 的 矩阵 BRc 是 从 Sr 到 Br 的 变换 矩阵 : 
0. 766 0 0. 643 
Br =BRGor =| 0.557 0.5 一 0.663 |r (2-210) 
一 0.322 0.866 0.383 
则 全 局 坐标 系 G (OXYZ) 中 的 位 置 矢量 cr 为 
0.766 0.557 —0.322)( 100 0. 45 
Gr 一 RbBr 一 BRIBr 一 | 0 0.5 0.866 || —50|=]104.9| (2-211) 
0.643 一 0.663 0.383 八 150 154. 9 
例 2-30 两 点 的 变换 矩阵 。 
刚体 B 上 两 点 Py 和 了， 的 全 局 位 置 矢量 分 别 为 


1. 077 一 0. 473 
Grp 一 |1. 365 Grp, 一 | 2.239 (2-212) 

2. 666 一 0. 959 
刚体 坐标 系 BCOzyz) 的 原点 固定 在 全 局 坐标 系 GCOXYZ) 的 原点 上 ， 并 且 点 Pi 和 








P， 分 别 位 于 BCOzyz) 的 zx 轴 和 yy 轴 。 
为 了 求 得 8:Rs， 利用 局 部 单位 矢量 6? AIS] 获得 6k 








、 0. 338 
iz Gy 1 
Gj=— "=| 0. 429 (2-213) 
Irr | 0. 838 
: 一 0. 191 
Crp, 
Gj=—.—_ =| 0.902 (2-214) 
|Srp, | 
一 0. 387 
0. 338 —0.191 —0. 922 
G=G XG =]|0.429|Xx] 0.902 |=|—0. 029 (2-215) 
0. 838 —0. 387 0. 387 


因此 ， 两 点 Pi 和 了 P， 的 坐标 变换 和 矩阵 为 
0.338 一 0.191 一 0.922 
SRg=(Si Sj] SA) 一 |0.429 0.902 一 0.029 (2-216) 
0.838 一 0.387 0.387 











例 2-31 位 置 矢量 的 长 度 不 变量 。 
利用 旋转 矩阵 描述 不 同 坐 标 系 中 的 同一 矢量 ， 这 并 不 影响 矢量 的 长 度 和 方向 性 。 因 此 ， 


矢量 长 度 是 一 个 不 变量 ，。 














|r| =|or|=|8r| (2-217) 





长 度 不 变性 说 明 如 下 : 

|r |2=Sr -TGr= (GRpB r)TGRpBr= BrTGRIGR Bp =BrTBy (2-218) 
i] 2-32 i, j 和 的 斜 对 称 和 矩阵 。 
与 矢量 a 相对 应 的 斜 对 称 矩 阵 a 定义 如 下 : 


0 —az a2 
a=! a3 0 一 Q1 (2-219) 
—ae ay 0 
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因此 ， 有 
0 0 0 
i=|0 0 一 1 (2-220) 
0 1 0 
0 0 1 
j=|90 0 0 (2-221) 
= 0) 0 
0 = 0 
k=|1 0 0 (2-222) 
0 0 0 
例 2-33 欧 拉 角 旋 转 和 矩阵 的 逆 和 矩阵 。 
旋 进 、 章 动 、 自 转角 或 者 欧 拉 角 旋 转 和 矩阵 式 (2-106): 
cocy —chspsy cWSP 十 cgOcpSW ssy 
BAG=A.,yAi,0hA:.o=|—cpsy—cOcysp —sesp+cdcec 四 (2-223) 
sso — cos c0 
必须 被 转换 为 能 够 使 刚体 局 部 坐标 映射 至 全 局 坐标 的 变换 矩阵 。 
cocy—cOsgsp —cesd—cbcysp  sOse 
CAB =PAG =A Al pAs.o=|cpsetcicpsp —spshtcicecep —cpsd| (2-224) 
SOSW sO cw c0 
变换 矩阵 式 (2-223) PER A Fa HB WC FA E Pe AE E, IN (2-224) 被 称 为 全 局 欧 拉 旋转 





HEE, 

% Gil 2-34 变换 的 群 特性 。 

MRES S 满足 下 列 的 4 个 公理 ， 那 么 具有 由 集合 S 中 的 元 素 所 定义 的 二 元 操作 多 的 
集合 S 被 称 为 一 个 群 (S， O). 

1) 闭 包 性 (Closure): 如 果 s1、s2E€ES，,， 则 有 s1C@s2 ES。 

2) 同一 性 Cidentity): 对 于 VsES， 存 在 一 元 素 使 得 sos =s =s. 

3) BPE Inverse); 对 于 每 个 ES， 总 存在 唯一 逆 s-!1ES 使 得 5s0C@9s 1 二 ;1C9s0 王 $0。 

4) 结合 性 (Associativity): We si. s2, s3€S. 则 (51@s2) Os; =s: (5200s3)。 

如 果 我 们 利用 下 式 定义 旋转 矩阵 的 集合 ， 那 么 三 维 坐 标 变 换 就 可 形成 一 个 群 。 

S={RER**?, RRT=RTR=I, |R|=1} (2-225) 

因此 ， 集 合 S 中 的 元 素 是 变换 矩阵 R;， 二 元 算 子 表示 旋 矩 相 弱 ,单位 矩阵 是 I， 并 且 
元 素 R 的 逆 和 矩阵 满足 R-! 二 RT。 

二 元 矩阵 相 乘 是 一 个 连续 操作 。 

集合 S 中 任何 元 素 的 逆 是 该 元 素 的 一 个 连续 函数 。 

因此 ， 集 合 S 是 一 个 微分 流 形 。 一 个 微分 流 形 的 群 被 称 为 李 群 。 

友 例 2-35 具有 行列 式 一 1 的 变换 。 
具有 行列 式 十 1 的 一 个 正 交 矩阵 对 应 着 式 (2-202) 中 所 描述 的 一 个 旋转 。 相 比 之 下 ， 
用 有 具有 行列 式 一 1 的 一 个 正 交 矩阵 描述 一 个 映像 ， 而 且 它 将 右手 定 则 坐标 系统 转换 成 左手 定 
则 系统 ， 反 之 亦 然 。 这 种 变换 并 不 相当 于 刚体 上 任何 可 能 的 物理 动作 。 
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例 2-36 ”变换 矩阵 的 另 一 种 证 明 。 
从 一 个 单位 开始 ， 有 











(i j Bj l=1 (2-226) 
È 
可 以 写 为 
I i 
Â J KO=|F IG 7 Ba (2-227) 
K È 
因为 可 以 按照 任何 次 序 进 行 和 矩阵 相 乘 ， 因 此 可 以 求 得 
Tei fej TAN i 
Gd f Boy=|fei fej Sek lp |=oRelj (2-228) 
Kei Ke? RRJ) È 
这 里 
Î Tei fej TAN 
SRe=|J |G 7 H=l|Fei Seg Sek ly (2-229) 
K Kei K+ej KekJ\k 
按照 同样 的 办 法 ， 可 以 求 得 : 
i 
BRo=|j |G f R) (2-230) 


k 


2.10 主动 变换 和 被 动 变换 





当 点 P 的 位 置 矢量 sr 固定 在 全 局 坐标 系 中 且 不 随 着 局 部 坐标 系 一 起 旋转 时 ， 局 部 坐标 
系 的 旋转 被 称 为 被 动 变换 ， 当 点 卫 的 位 置 矢 量 sr 固定 在 局 部 坐标 系 中 并 随 着 局 部 坐标 系 一 
起 旋转 时 ， 局 部 坐标 系 的 旋转 被 称 为 主动 变换 。 让 人 感到 意外 的 是 ， 被 动 变换 和 主动 变换 在 
数学 上 是 等 效 的 。 换 句 话说 ， 对 于 一 个 旋转 坐标 系 和 旋转 矢量 (主动 变换 ) 的 旋转 矩阵 及 对 
于 一 个 旋转 坐标 系 和 固定 矢量 (被 动 变 换 ) 的 旋转 矩阵 来 讲 ， 是 一 样 的 。 

证 明 : 

相对 于 固定 坐标 系 G(OXYZ)， 考虑 被 旋转 的 局 部 (刚体 ) 坐标 系 BCOzyz)， 如 
图 2-19 所 示 。 点 了 是 全 局 坐标 系 中 的 一 个 固定 点 ， 它 的 全 局 位 置 矢量 为 sr 。 点 卫 MER 
量 能 够 在 一 个 局 部 坐标 系 或 者 全 局 坐标 系 中 被 分 解 ， 分 别 用 3?r 和 Sr 表示 。 

从 Sr 到 Br 的 变换 等 效 于 与 全 局 坐标 系 G(OXYZ) 相 重 合 的 局 部 坐标 系 B(Ozyz) 所 要 
求 的 旋转 。 这 是 一 个 主动 变换 ， 因 为 局 部 坐标 系 并 不 能 移动 矢量 sr。 在 被 动 变换 中 ， 通 
常 知 道 点 尸 在 全 局 坐标 系 中 的 坐标 ， 需 要 的 是 它 在 局 部 坐标 系 中 的 坐标 。 因 此 ， 使 用 
下 列 方程 : 












































51 


As: 运动 学 、 动 力学 与 控制 技术 

















图 2-19 局 部 和 全 局 坐标 系 中 的 位 置 矢量 r 





Br 一 BRccr (2-231) 
另外 ， 我 们 可 以 假设 局 部 坐标 系 BC Oxyz) 与 全 局 坐标 系 GCOXYZ) MBA. 在 也 
(Oxyz) 与 Br 移动 至 全 局 坐标 系 GOOXYZ) 中 的 新 位 置 之 前 ， 矢 量 r =r 固定 于 局 部 坐标 
系 BCOzyz)。 这 是 一 个 主动 变换 ， 并 且 有 这 样 一 个 旋转 变换 矩阵 能 将 局 部 坐标 系 中 Br 的 坐 
标 变换 到 全 局 坐标 系 中 Sr 的 坐标 。 在 主动 变换 中 ， 通 常 知 道 局 部 坐标 系 中 点 P 的 坐标 ， 需 
要 的 是 它 在 全 局 坐标 系 中 的 坐标 。 因 此 ， 通常 使 用 下 列 方程 : 
Gr 一 CRBBr (2-232) 
例 2-37 绕 X 轴 的 主动 旋转 和 被 动 旋 转 。 
考虑 重合 的 局 部 坐标 系 B 和 全 局 坐标 系 G. MES P 位 于 矢量 sr 处 。 























1 
Br =|2 (2-233) 
1 
BEX 轴 旋 转 45 将 点 P 移动 到 矢量 sr 
1 0 0 
x o nifl 1 
Gr =Rx so Br = oe Sn |e |= 11 (2-234) 
1 2 
0 Sin 一 cos 一 


现在 假设 点 PP 固定 于 全 局 坐标 系 GIR. Akin KR B 绕 全 局 坐标 系 的 X 轴 旋 转 90 ， 则 
局 部 坐标 系 中 点 P 的 坐标 为 





0 T ; T 1 

+ cos sin 

Br =Ry.-90° Sr = 2 2 2|= 1 (2-235) 
1 
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2. 11 hák 


本 章 的 学 习 目 标 包括 : 

1) 学 习 怎 样 通过 应 用 绕 基 本 轴 的 旋转 来 确定 在 具有 共同 原点 的 两 直角 坐标 系 之 间 的 旋 
转变 换 和 矩阵 。 

2) 将 一 个 给 定 的 变换 矩阵 分 解 为 一 系列 所 要 求 的 基本 旋转 。 
具有 共同 原点 的 两 个 笛 卡 儿 坐 标 系 B 和 G 通过 另 一 个 坐标 系 中 的 9 个 方向 余弦 相关 联 。 
两 个 坐标 系 中 的 坐标 变换 在 矩阵 变换 中 可 以 被 转换 。 


























Sr 一 CRBBr (2-236) 
X2 Tei [ej Tek)(x2 
YoJ=|Jei Jeg Sek liye (2-237) 
Z2 Kei Kej Kekj\xe 


we 
HE 


cos(Î,i) cos(Î,j) cos(Î,$) 
GRp=|cos(j ,i) cos(j ,7) cos(J ,k) (2-238) 
cos(K,i) cos(K.j) cos(K,k) 
变换 算 转 %*Rs 是 正 交 的 ， 因 此 它 的 行列 式 的 值 是 1， 同时， 它 的 道 矩 阵 等 于 它 的 转 置 
矩阵。 





|SRs |=1 (2-239) 
SR, 一 GRE (2-240) 
TE 20 2 (FFE ERIE OR» 元 素 之 间 产 生 6 个 方程 ， 变 换 和 矩阵 SRp 表示 其 中 仅仅 有 3 个 
元 素 是 独立 的 。 
局 部 坐标 系 B 在 全 局 坐标 系 G 中 的 任何 相关 定位 通过 绕 局 部 坐标 系 B 或 者 全 局 坐标 系 
G 中 坐标 轴 的 3 个 连续 旋转 实现 。 如 果 局 部 坐标 系 B 是 刚体 坐标 系 ， 并 且 全 局 坐标 系 G 是 
一 个 全 局 固定 坐标 系 ， 那 么 全 局 基本 旋转 变换 矩阵 为 
1 0 


























Rx.y=°Rp=|0 cosy —siny (2-241) 
0 siny cosy 
cosB 0 sinf 
Ry.s=° Rs= 0 1 0 (2-242) 
一 Sin8 0 cosB 
cosa —sina 0 
Rz.c=°Rp=| sina cosa 0 (2-243) 
0 0 1 
刚体 的 基本 旋转 变换 矩阵 为 
1 0 0 
ac cos) 可 (2-244) 
0 一 SinW cosy 
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cos? 0 一 Sin0 
有 ,0 王 BRc 王 | 0 1 0 
sind 0 cosO 
cosp sing 0 
R..s=?Re=|—sing cosp 0 
0 0 1 
全 局 旋转 变换 和 局 部 旋转 变换 是 互 逆 的 。 
Rx =R], 
Ry.e=R5.p 
Rza =R}, 
习题 
2-1 标记 与 符号 。 
描述 下 列 这 些 符号 的 含义 。 
Dor, 2)Srp、 3)8rp、 4) SR 5) RE 
7) BRG! 8) dy 9) 2di 10) Qx 11) Qy.g 
IDÉ ws 15)47。16)2 Di 
2-2 刚体 点 和 全 局 旋转 。 
点 卫 在 刚体 坐标 系 B (Oryz) 中 位 于 Brp 一 (1 2 


P 的 最 终 全 局 位 置 。 
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1) RE X 轴 旋 转 30"， 接 着 绕 着 Z 轴 旋 转 45°. 
2) 绕 着 Z 轴 旋 转 30 ， 接 着 绕 着 X 轴 旋 转 45°, 
WS) A P 在 球面 上 移动 。 假 设 点 P 的 全 局 初始 位 置 由 P 





(2-245) 


(2-246) 


(2-247) 
(2-248) 
(2-249) 


6) RG 
12) @y 


18) I 


DT 处 。 进 行 下 列 旋转 之 后 ， 求 点 


1 表示 ， 第 2 个 位 置 由 Po K 


第 3 个 位 置 由 Ps 表示 。 试 确定 人 P10P，。、 人 POP3 和 人 P30OPi1。 


Kw) 求 由 点 Srp 构成 的 三 角形 的 面积 ， 以 及 点 已 旋转 1) 


友 2-3 到 达 一 方位 的 另 一 种 运动 。 

在 刚体 坐标 系 B 和 全 局 坐标 系 G 中 , 刚体 点 P 的 坐标 为 
1. 23 4. 56 

Brp 一 |4.56| Srp 一 |7.89 
7. 89 1; 23 

确定 : 

1) 是 否 有 可 能 将 矢量 srP 转换 到 矢量 srP? 


2) 
3) 
4) 
5) 从 矢量 ?rp BARES rp AY aah aR AA MPA 
2-4 全 局 旋转 之 后 的 刚体 点 。 


TERE rp MARES rp 之 间 的 变换 矩阵 。 
从 矢量 ?rp IRES rp 的 欧 拉 角 。 














从 矢量 Srp IRE re 的 全 局 翻滚 角 、 俯 仰角 、 侦 航 角 。 


和 2) 之 后 的 全 局 位 置 。 
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局 部 坐标 系 中 一 点 P 在 进行 下 面相 应 的 旋转 之 后 ， 如 果 移 动 到 Srp 二 (1 3 2)T Ab, 
那么 试 求 点 P 在 局 部 坐标 系 中 的 位 置 。 

1) 绕 着 Z 轴 旋 转 60°. 

2) RA X 轴 旋 转 60°. 

3) RA Y 轴 旋 转 60°, 

4) RA Z 轴 旋 转 60"， 接 着 绕 着 X 轴 旋 转 60"， 最 后 绕 着 Y 轴 旋 转 60°. 

2-5 矢量 不 变量 。 

在 矢量 arp= (1 2 z) ÆSA P, BRE X 轴 旋转 60"， 接 着 绕 着 Z 轴 旋 转 
30" 之 后 ， 位 于 


X 
crp 一 Y 
2. 933 


求 >、X MY, 
2-6 ”立方 体 的 全 局 旋转 。 
2-20 所 示 为 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 棱 长 ! 王 1。 














=N 











图 2-20 初始 位 置 处 的 立方 体 

试 确定 : 
1) RE X 轴 旋 转 30" 之 后 的 顶点 坐标 。 
2) Se Y 轴 旋 转 30" 之 后 的 顶点 坐标 。 
3) RA Z 轴 旋 转 30" 之 后 的 顶点 坐标 。 
4) RE X 轴 旋 转 30°, RERA Y 轴 旋 转 30, MAE 2 轴 旋 转 30" 之 后 的 顶点 坐标 。 
2-7 对 固定 长 度 的 矢量 。 
证 明 : 矢量 长 度 并 不 随 着 旋转 而 改变 。 

[Sr |= |CRp?r | 
证 明 : 两 刚体 点 之 间 的 距离 并 不 随 着 旋转 而 改变 。 
pi1 一 5p2 | 一 |5Raspl 一 5RBBP2 | 
2-8 重复 全 局 旋转 。 
将 矢量 Srp 二 (2 2 3T Se X 轴 旋 转 60 ， 接 着 绕 着 Z 轴 旋 转 30"。 这 时 重复 旋转 矢 
量 所 旋转 的 顺序 ( 即 绕 着 X 轴 旋 转 60"， 重 复 接着 绕 着 Z 轴 旋 转 30")。 重 复 旋 转 多 少 次 后 
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A P 回 到 它 最 初 的 全 局 位 置 ? 

W2-9 重复 全 局 旋转 。 

绕 着 X He a=180°/m, ， 接 着 绕 着 Z MEH B= 180°/n, WA m, nEN, MERZ 
少 次 后 能 将 一 个 刚体 点 带 回 至 它 的 初始 全 局 位 置 ? 

2-10 三重 全 局 旋转 。 

验证 附录 A 中 的 方程 。 

六 2-11 特殊 的 三 重 旋转 。 

假设 附录 A 中 第 1 个 三 重 旋 转 将 刚体 点 带 回 到 它 的 最 初 全 局 位 置 。 试 求 旋转 角 ， 其 中 
Q 产 0、pB 产 0、7Y 妆 0。 

W212 三 重 旋转 的 综合 。 

附录 A 中 任何 三 重 旋转 可 以 将 一 个 刚体 点 移动 到 它 的 新 的 全 局 位 置 。 假 设 对 于 情况 1) 
Qx. QYZ ais k 和 Yi 已 知 。 依 据 a1、B1 Aly. AAT 2) Qx.7,Qy.p,Qz.0 + 
“az. Bo MY. 分 别 是 多 少 度 时 也 能 到 达 同 一 全 局 位 置 ? 

2-13 全 局 翻滚 -俯仰 - 偏 航 旋转 和 矩阵。 

绕 主轴 旋转 30" 时 计算 全 局 翻滚 -俯仰 - 偏 航 矩阵 CQ 和 局 部 翻滚 -俯仰 - 偏 航 矩阵 和 4。 通过 
z 轴 然 后 绕 y 轴 最 后 再 绕 过 轴 旋 转 计算 局 部 旋转 矩阵 A。 新 矩阵 A 的 转 置 就 是 全 局 翻滚 - 估 
仰 - 偏 航 矩阵 的 转 置 矩 阵 吗 ? 

2-14 ”全 局 翻 深 -俯仰 - 偏 航 旋转 角 。 

对 于 下 列 的 旋转 和 矩阵， 计算 其 翻滚- 俯仰 - 偏 航 旋转 角 。 
0.53 一 0. 84 a 
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BRG=| 0.0 0.15 0.99 
—0.85 —0.52 0.081 

k 2-15 返回 最 初 的 方位 和 附录 A. 

假设 利用 附录 A 中 的 第 1 组 公式 ， 旋 转 刚 体 B。 通 过 分 别 应 用 下 列 各 组 公式 ， 怎 样 才能 
将 刚体 返回 至 它 的 最 初 方位 ? 

1) 附录 A 中 的 第 1 组 公式 。 

2) 附录 A 中 的 第 2 组 公式 。 

3) 附录 A 中 的 第 3 组 公式 。 

x4) 假设 利用 附录 A 中 的 第 1 组 公式 已 经 将 刚体 B 旋转 了 au = 30°, Bi 530M y = 
30"。 我 们 想 将 B 返回 它 的 原始 方位 。 在 附录 A 的 第 2 组 、 第 3 组 公式 中 ， 哪 一 组 能 较 快 地 
实现 ?我 们 认为 最 快 的 那 组 是 具有 s =a: +8: +y: 最 小 值 的 那 一 组 。 

W216 返回 最 初 的 方位 和 附录 B。 

假设 利用 附录 A 中 的 第 1 组 公式 ,旋转 刚体 B。 通 过 分 别 应 用 下 列 各 组 公式 ， 我 们 怎样 
才能 将 刚体 返回 至 它 的 最 初 方位 ? 

1) 附录 B 中 的 第 1 组 公式 。 

2) 附录 B 中 的 第 2 组 公式 。 

3) 附录 B 中 的 第 3 组 公式 。 

KA) 假设 利用 附录 A 中 的 第 1 组 公式 我 们 已 经 将 刚体 B 旋 转 了 a 二 30"、B 二 30" 和 Y= 
30"。 我 们 想 将 B 返回 其 原始 方位 。 在 附录 B 的 第 1 组 、 第 2 组 、 第 3 组 公式 中 ， 哪 一 组 能 
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较 快 地 实现 这 一 目标 ? 我 们 认为 最 快 的 那 组 是 使 *=p 十 9 十 少 具 有 最 小 值 的 那 一 组 。 
2-17 两 个 局 部 旋转 。 
试 求 位 于 Brp 二 (2 2 DT 处 的 刚体 点 在 进行 下 列 旋转 之 后 的 全 局 坐标 。 
1) 绕 着 x 轴 旋 转 60"， 接 着 绕 着 z 轴 旋 转 60°. 
2) 绕 着 > 轴 旋 转 60"， 接 着 绕 着 x 轴 旋 转 60°, 
3) SEA > 轴 旋 转 60"， 接 着 绕 着 x 轴 旋 转 60"， 最 后 再 绕 着 > 轴 旋 转 60°. 
2-18 立方 体 局 部 旋转 
2-20 所 示 为 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 校长 !“ 王 1。 试 确定 : 
D 绕 着 x 轴 旋 转 30" 之 后 的 顶点 坐标 。 
2) RE y 轴 旋 转 30" 之 后 的 顶点 坐标 。 
3) 绕 着 > 轴 旋 转 30" 之 后 的 顶点 坐标 。 
4) 绕 着 x 轴 旋 转 30"， 再 绕 着 y 轴 旋 转 30"， 最 后 绕 着 z 轴 旋 转 30" 之 后 的 顶点 坐标 。 
2-19 立方 体 的 全 局 旋转 和 局 部 旋转 。 
2-20 所 示 为 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 棱 长 !“ 王 1。 试 确定 : 
1) RÉ x 轴 旋 转 30°, RERA X 轴 旋 转 30" 之 后 的 顶点 坐标 。 
2) RA y 轴 旋 转 30"， 接 着 绕 着 X 轴 旋 转 30" 之 后 的 顶点 坐标 。 
3) SEA z 轴 旋 转 30"， 接 着 绕 着 X 轴 旋 转 30" 之 后 的 顶点 坐标 。 
4) SES x 轴 旋 转 30"， 接 着 绕 着 X 轴 旋 转 30"， 再 绕 着 x 轴 旋 转 30" 之 后 的 顶点 坐标 。 
5) SE x 轴 旋 转 30"， 接 着 绕 着 Y 轴 旋 转 30"， 再 绕 着 = 轴 旋 转 30" 之 后 的 顶点 坐标 。 
2-20 刚体 点 的 局 部 旋转 。 
位 于 arp 王 (2 2 3)7 处 的 刚体 点 分 别 进行 下 面 的 旋转 之 后 ， 其 全 局 坐标 是 什么 ? 
1) 绕 着 x 轴 旋 转 60°, 
2) 绕 着 y 轴 旋 转 60°, 
3) 绕 着 > 轴 旋 转 60°. 
2-21 求 未 知 旋转 角 之 一 。 
绕 着 x 轴 旋 转 将 Srp 二 (2 2 3)T 变换 到 Srp 二 (2 Yp OT, WR Yp MERA. 
2-22 求 未 知 旋转 角 之 二 。 
考虑 位 于 Brp 一 (2 V3 V2) 处 的 一 个 点 已 。 试 确定 满足 如 下 条 件 的 旋转 角 。 
D KREZ X Y, Z 轴 进 行 所 要 求 的 主 全 局 旋转 ， 将 点 P R oh Sl 
Grp 一 (V2 2 43)", 
2) 依次 围绕 Z、Y、2Z 轴 进 行 所 要 求 的 主 全 局 旋转 ， 将 点 已 移动 到 
Grp 一 (2 2 43)", 
3) KERZ X Z 轴 进 行 所 要 求 的 主 全 局 旋转 ， 将 点 P 移动 到 
rp 一 (V2 2 rch 
2-23 局 部 三 重 旋 转 。 
验证 附录 B 中 的 方程 。 
2-24 局 部 和 全 局 的 综合 旋转 。 
试 求 位 于 Srp 二 (10 10 一 10)7 处 的 刚体 点 分 别 进 行 了 下 面 的 旋转 之 后 的 最 终 全 局 
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位 置 。 








的 旋 


少 次 
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1) SEA x 轴 旋 转 45”"， 接 着 围绕 Z 轴 旋 转 60°. 

2) 绕 着 z 轴 旋 转 45”"， 接 着 围绕 Z 轴 旋 转 60°. 

3) 绕 着 x 轴 旋 转 45"， 接 着 围绕 Z 轴 旋 转 45"， 再 围绕 X 轴 旋 转 60°. 

2-25 ”局 部 和 全 局 的 综合 旋转 。 

试 求 位 于 Srp 二 (10 10 一 10)7 处 的 刚体 点 分 别 进行 了 下 面 的 旋转 之 后 的 最 终 全 局 
































D 绕 着 X 轴 旋 转 45"， 接 着 围绕 > 轴 旋 转 60°. 

2) 绕 着 Z 轴 旋 转 45"， 接 着 围绕 > 轴 旋 转 60°. 

3) RE X 轴 旋 转 45"， 接 着 围绕 x 轴 旋 转 45"， 再 围绕 = 轴 旋 转 60°. 

2-26 重复 局 部 旋转 。 

将 矢量 Brp =(2 2 3)7 绕 着 z 轴 旋 转 60"， 接 着 再 绕 着 z 轴 旋 转 30"。 然 后 重复 这 样 
转 顺 序 。 旋 转 多 少 次 后 点 了 才能 回 到 其 最 初 的 全 局 位 置 ? 

友 2-27 重复 局 部 旋转 。 

绕 着 x Hee a=180°/m, RASA z HEH P=180°/n, MA m, nEN, MJERE 
后 能 将 一 个 刚体 点 移动 回 至 它 的 初始 全 局 位 置 ? 

妇 2-28 其 余 的 旋转 。 

试 求 下 列 旋转 顺序 的 结果 。 

















GRB =A} A] yA], 0 
2-29 来自 旋转 矩阵 的 旋转 角 。 
如 果 附 录 也 中 的 旋转 变换 矩阵 已 知 ， 试 求 旋 转角 P、0 和 y。 
2-30 来自 旋转 矩阵 的 欧 拉 角 。 
1) 检查 下 列 矩 阵 5Ra 是 否 是 一 个 旋转 变换 。 
0. 53 —0.84 0. J 








GRg=| 0. 0. 15 0. 99 
—0.85 —0.52 0.081 
2) 试 求 和 矩阵 SRa 的 欧 拉 角 。 
D 试 求 矩阵 "Ra 的 局 部 翻 深 角 、 信 仰角、 偏 航 角 。 
2-31 与 两 个 旋转 相当 的 欧 拉 角 。 
试 求 相 当 于 旋转 矩阵 的 欧 拉 角 。 
1) BRE =Ay.15Ax,30 6 

2) BRG=Ax,45Ay,30 0 

3) BRG=Ay,45A 2,30 0 

2-32 与 3 个 旋转 相当 的 欧 拉 角 。 
1) BRe=Az,60Ay,45Ax,30 

2) BRG=Az.60Ay.45Az.30 

3) BRE =Ax,60A y.45A4 2,30 0 

K 2-33 WIT PRAY eRe A = <0 的 严禁 空间 。 

2-20 展示 了 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 校长 1 二 1。 假 设 在 任何 时 
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候 所 有 顶点 都 不 允许 有 一 个 负 分 量 。 

1) 进行 一 系列 全 局 主 旋 转 运动 使 得 直线 FH 平行 于 > 轴 。 

2) 进行 一 系列 全 局 主 旋 转 运动 使 得 直线 DB 平行 于 = th. 并且 点 A 位 于 (Z, Y) 平 
面 内 。 

3) 进行 一 系列 局 部 主 旋 转 运动 使 得 直线 FH 平行 于 > 轴 。 

4) 进行 一 系列 局 部 主 旋 转 运动 使 得 直线 DB 位 于 > 轴 上 ， 并 且 点 A 位 于 (Z, Y) 平 
面 内 。 

友 2-34 局 部 位 置 和 全 局 位 置 ， 欧 拉 角 。 

试 求 欧 拉 角 之 间 的 条 件 。 

1) 将 矢量 Srp = 二 (1 1 0)7 变换 到 Br p= 二 (0 1 DT, 

2) 将 矢量 Srp 二 (1 1 0)7 变换 到 Brp 二 (1 0 17, 

W235 与 一 个 三 重 旋 转 相 当 的 欧 拉 角 。 

试 求 附录 B 中 下 列 情况 旋转 矩阵 的 欧 拉 角 。 

4—AzyiAy,/Az,o' 

W236 ” 欧 拉 频率 的 可 积分 性 。 

证 明 : 如 果 @ 和 y 是 第 1 和 第 3 欧 拉 角 ， ABA do 和 dy 是 可 积分 的 。 

2-37 ”对 于 欧 拉 角 的 卡尔 丹 角 。 

1) 已 知 一 组 欧 拉 角 ， 试 求 卡尔 丹 角 。 

2) 已 知 一 组 卡尔 丹 角 ， 试 求 欧 拉 角 。 

W2-38 对 于 欧 拉 频率 的 卡尔 丹 频率 。 

1) 根据 卡尔 丹 频 率 ， 试 求 欧 拉 频 率 。 

2) 根据 欧 拉 频 率 ， 试 求 卡 尔 丹 频 率 。 

WK2-39 变换 矩阵 和 3 次 旋转 。 

图 2-20 展示 了 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 棱 长 “=1。 假 设 在 新 的 方 
位 ， 点 DD 和 点 下 位 于 x 轴 上 ,点 A 位 于 (X，2Z) 平面 内 。 试 确定 : 

1) 初始 方位 和 新 方位 之 间 的 变换 矩阵 。 

2) 将 立方 体 移 动 到 新 方位 的 欧 拉 角 。 

D 将 立方 体 移动 到 新 方位 的 全 局 翻滚 角 、 俯 仰角 、 偏 航 角 。 

4) 将 立方 体 移 动 到 新 方位 的 局 部 翻滚 角 、 俯 仰角 、 偏 航 角 。 

k2-40 另 一 种 替代 方法 。 

图 2-20 展示 了 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 棱 长 2“ 王 1。 假 设 在 新 的 方 
位 中 ， 点 DD 和 点 下 位 于 z 轴 上 ， KAMT (X, Z) 平面 内 。 试 确定 : 

1) KEY, X, Z 轴 旋 转 分 别 作 为 第 1 次、 第 2 次、 第 3 次 旋转 ， 试 求 旋 转角 。 

2) HERY, Z, X 轴 旋 转 分 别 作 为 第 1 次、 第 2 次 、 第 3 次 旋转 ， 试 求 旋转 角 。 

3) 将 围绕 vy. x. z 轴 旋 转 分 别 作 为 第 1 次、 第 2 次、 第 3 次 旋转 ， 试 求 旋转 角 。 

4) 将 围绕 vy. xe. x 轴 旋 转 分 别 作为 第 1 次、 第 2 次、 第 3 次 旋转 , 试 求 旋转 角 。 
5) WRB y, Z, x 轴 旋 转 分 别 作 为 第 1 次、 第 2 次 、 第 3 次 旋转 ， 试 求 旋转 角 。 

































































































































































O 将 围绕 Y、z、X 轴 旋 转 分 别 作 为 第 1 次、 第 2 次 、 第 3 次 旋转 ， 试 求 旋转 角 。 
D 将 围绕 x、X、z 轴 旋 转 分 别 作为 第 1 次、 第 2 次、 第 3 次 旋转 ， 试 求 旋转 角 。 
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2-41 旋转 矩阵 中 的 元 素 。 
旋转 矩阵 "Ra 中 的 元 素 是 

cos(Î, i) cos(Î, j) cos(I, k) 

SRg=|cos(J, i) cos(J, j) cos(Î, &) 

cos(I, 7) cos(Î, j) cos(Î, k) 

如 果 矢量 Srp， = (0.7071 一 1.2247 1.4142)? 位 于 x 轴 上 ， 矢 量 srp = 
(2.7803 0.38049 一 1.0607)7 位 于 yy 轴 上 ， 试 求 旋 转变 换 矩 阵 GRa 。 

2-42 ”线性 独立 变量 。 
如 果 一 组 矢量 使 下 列 方程 成 立 ， 那 么 该 组 矢量 cl az, ty an 被 认为 是 线性 独立 的 。 

kiai Fkzag 十 十 kjan 二 0 
TET TEP. Riv Rov ory ka 是 未 知 系数 ， 仅 有 唯一 解 。 

ki 一 & =" =k, =0 
验证 : 表达 在 全 局 坐标 系 GCOXYZ) 中 的 刚体 坐标 系 BCOxyz) 单位 矢量 是 线性 独 
立 的 。 

2-43 TE SCRE MEM FEAR. 
XFER, WRR SRT, PRT) 一 Ri ， 则 该 矩阵 被 称 为 正 交 和 矩阵 。 
证 明 : 两 正 交 算 阵 相 乘 之 后 的 矩阵 也 是 正 交 和 矩阵 。 
2-44 矢量 恒等式 。 
对 于 标量 ,公式 (a 十 6)? 二 a? 十 b? 十 2ab 相当 于 

(atb)?=a+atbh+b+2a+b 
对 于 矢量 ， 证 明 该 公式 等 于 

(a 二 D)2 一 aa :bi2°Rea +b 

如 果 a 是 局 部 坐标 系 中 的 一 个 矢量 ,，b 是 全 局 坐标 系 中 的 一 个 矢量 。 
2-45 ”旋转 作为 线性 操作 。 
EAH ; 





















































R (a Xb)=Ra XRb 

这 里 ，R 是 一 个 旋转 和 矩阵，a Mb 是 定义 在 坐标 系 中 的 两 个 矢量 。 

2-46 ”标量 三 重 积 。 

WH: 对 于 任意 矢量 a 、b Alle, A 

a ° (b Xc)=(a Xb)*ce 

2-47 欧 拉 角 和 最 小 距离 。 

2-20 所 示 为 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 棱 长 /二 1。 假 设 在 新 的 方 
位 中 ， 点 DD 和 点 下 位 于 z 轴 上 ， 点 A 位 于 (X，Y) 平 面 内 。 试 确定 : 

D 最 初 方位 和 新 方位 之 间 的 变换 矩阵 。 

2) 将 立方 体 移 动 到 新 方位 的 欧 拉 角 。 

3) 选择 3 个 非 共 面 的 顶点 ， 利 用 未 知 欧 拉 角 的 欧 拉 变换 矩阵 确定 它们 的 位 置 。 定 义 点 
的 最 初 位 置 和 最 终 位 置 之 间 的 距离 为 di、cd 和 qd。 有 可 能 利用 距离 目标 函数 
J 三 d1 十 di 十 d3 最 小 化 确定 欧 拉 角 吗 ? 
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友 2-48 ”连续 欧 拉 角 。 
2-20 所 示 为 在 D 处 具有 固定 点 的 立方 体 的 初始 位 置 ， 它 的 楼 长 1! 二 1。 假 设立 方 体 以 
角速度 oil RE x 轴 旋 转 ， 同 时 以 角速度 ws 绕 着 Z 轴 旋 转 。 试 确定 点 下 的 运动 路 径 。 如 果 


0i=0:., @=20:, @ =30:, @ı 二 4@，， 该 运动 路 径 是 什么 ? 
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可 以 将 任何 具有 固定 点 O 的 刚体 绕 全 局 固定 坐标 轴 旋转 角 8 分 解 成 绕 着 3 个 给 定 的 
非 共 面 轴 的 3 个 旋转 。 而 且 ， 在 有 限 次 数 的 旋转 之 后 ， 刚 体 的 最 终 定向 等 效 于 绕 着 一 个 特定 
轴 所 进行 的 一 个 特定 的 旋转 。 确 定 刚体 的 旋转 角度 和 轴 称 为 刚体 的 定向 运动 。 


3.1 轴 角 旋转 


有 必要 用 两 个 参数 定义 一 条 通过 O 点 的 直线 方向 ， 有 必要 用 一 个 参数 定义 刚体 绕 这 条 
直线 所 旋转 的 角度 。 使 刚体 坐标 系 B (Ozyz) 绕 着 具有 方向 余弦 ui, uz, us 的 单位 矢量 ù 
所 说 明 的 一 条 直线 旋转 $ 角 。 





u=uil+u2t tu3K (3-1) 
Ju? +u? +u =1 (3-2) 


这 称 为 角 轴 旋转 的 表示 。 
变换 矩阵 5RE 能 将 局 部 坐标 系 BC(COzyz) 中 的 坐标 映射 至 全 局 坐标 系 GCOXYZ) 中 的 
相应 坐标 。 























Sr 一 SRBBr (3-3) 
该 旋转 变换 矩阵 SRb 是 : 
SRB 王 Roy 一 Icosg uu Tversg tusing (3-4) 
ui vers cy uluzversdé—u3sp uius3vers$ Tu2sg 
SRE=|uiu2sversg tuss¢ u3versg cy u2u3vers$ — us$ (3-5) 
ulugversdé—u2sp uu2us3versg Tuisg usversg +c 
这 里 
vers¢ =1—cos¢ =2 sin? $ (3-6) 


并 且 , u EGA ù 相对 应 的 斜 对 称 和 矩阵 。 
0 — u3 U2 
"| U3 0 5 (3-7) 
—u2 Ul 0 


ul=—u (3-8) 


如 果 
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BBA SE BE u 就 是 斜 对 称 的 。 

对 于 相对 于 全 局 坐标 系 的 局 部 坐标 系 旋转 ， 式 (3-5) 中 的 旋转 变换 矩阵 是 广义 旋转 矩 
阵 。 如 果 式 (3-1) 的 旋转 坐标 轴 与 全 局 坐标 轴 相 重合 ， 则 将 再 现 式 (2-20)、 式 (2-21) 
或 者 式 (2-22), 

给 定 一 个 变换 矩阵 Rb ， 能 够 通过 下 式 获 得 旋转 的 轴 u 和 角度 y。 


























pA 1 G —GRT = 
u= Fang (ORB ORD) (3-9) 
1 f 
cosg =; (Ctr Ra) — 1) (3-10) 
证 明 : Žž z 




















有 趣 的 是 ， 绕 一 个 轴 u 的 旋转 效果 等 效 于 绕 | 
着 局 部 坐标 系 轴 所 进行 的 一 系列 旋转 ， 局 部 坐标 时 
系 首 先 旋转 至 其 中 的 一 个 轴 ， 我 们 称 之 为 > 轴 ， 
使 之 与 旋转 轴 w 重合 ， 紧 接着 绕 局 部 轴 旋 转 F 角 ， 
这 是 最 初 旋转 序列 的 逆转 。 

图 3-1 说 明了 ， 当 局 部 坐标 轴 > 轴 与 x 重合 
时 ， 全 局 坐标 系 G(OXYZ) 以 及 被 旋转 的 局 部 坐 
标 系 B (Oryz) 旋转 了 xw 一 zz 十 xx? 了 十 x3 玉 。 基 
于 图 3-1， 局 部 坐标 系 BCOzyz) 先 绕 > 轴 旋 转 p 
角 ， 后 绕 y 轴 旋 转 0 角 ， 使 局 部 zx 轴 跟 旋转 轴 z À 
重合 ， 接 着 绕 u 旋转 风 角 ， 这 时 进行 反 向 序列 旋 图 3-1 SBA as = 轴 重 合 时 的 旋转 轴 
转 。 因 此 ， 利 用 式 (2-174), Bu eH 6 角 之 
后 将 局 部 坐标 转换 至 全 局 坐标 的 旋转 矩阵 5 Rs 为 : 

CRe=?Ro! =PRE =R., =(Az,-gAy,—0A2,gAy0Az.9) =A AT AT AT, AT 







































































一 9 
(3-11) 
但 是 ， 
: U2 ul 
sing = — cosg = — 
Jaret Satu D 
sinf = Ju? Fui cosl =u3 
sind sing = u2 sindcosp =u 
因此 ， 有 
uj versé +c¢ uj uzversdé—uUu3sShd uius3versg Tu2sy 
ERg =R., =| uiuzversd Huss us vers cy ususversg —U1S¢ (3-13) 
ujuzversé—u2sh u2usversd+uis¢d usversd+cd 
式 (3-13) 可 被 分 解 为 
1 0 0 ul 0 —u3 u? 
R.. =cos¢}0 1 O}+(1~cos¢d)|u2}(ui uz us)tsing| us 0 一 &1 
0 0 1 u3 Tuz? ul 0 
(3-14) 
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sk (3-14) 可 化 为 
SRep=R,,y—=Icosg Huu lversg using (3-15) 
式 (3-4) 被 称 为 罗 德 里 格 斯 (Rodriguez) 旋转 公式 ， 也 被 称 为 欧 拉 -莱克 塞 尔 - 罗 德 里 
格 斯 (Euler-Lexell-Rodriguez) 公式 。 
为 了 说 明 式 (3-9) 和 式 〈3-10) ， 扩 展 cRa 一 RE， 以 确定 旋转 矢量 性 的 轴 : 








0 —2(sind)u3  2(sing)u2 
GRB 一 RS 一 | 2(sind)us 0 —2(sing )ui (3-16) 
—2(sing )ue2 2(sing )ui 0 


求 取 旋 转 和 矩阵 "Ra 的 迹 ， 以 便 获 得 旋转 $ 角 : 
tr(°Rp) =ri +r Frs =3cos¢ Hui (1—cos¢) +u3(1—cos¢) +u3 (1 —cos¢) 














=3cos¢ tui Hus tus — (ui Hu} tu? )cosg =2cosġ +1 (3-17) 
罗 德 里 格 斯 旋转 公式 总 在 相关 参考 文献 中 以 下 列 的 等 效 形 式 被 报道 ; 
R.. =I (sing)u (vers )u? (3-18) 
Ra. = A au" )cosd +u sing taut (3-19) 
R.. =u’ cosg tusing Hu’ +1 (3-20) 
Ff E Fe HY DE FE E A 
BR 一 SRS 一 R yy =Icosd Húú" versé — using (3-21) 








这 意味 着 ， 当 局 部 坐标 系 BAREH u 旋转 8 角 时 B 在 全 局 坐标 系 G 中 的 定位 跟 当局 
部 坐标 系 B 绕 矢 量 轴 六 旋转 一 $ 角 时 G 在 局 部 坐标 系 B 中 的 定位 一 样 的 。 
3X3 实 正 交 变换 和 矩阵 R 也 被 称 为 转动 体 ， 反 斜 对 称 矩 阵 u 被 称 为 旋 量 。 可 以 验证 








uu=0 (3-22) 

I—úúT =u? (3-23) 

rur =0 (3-24) 

ú Xr =ur=—ru=—r Xu (3-25) 





证 毕 。 
例 3-1 “4 u=K 时 的 轴 角 旋转 。 
如 果 局 部 坐标 系 BC(Ozxyz) SEZ 轴 旋 转 时 ， 则 
u=K (3-26) 








这 时 变换 矩阵 (3-5) 简化 为 

Oversg Hcos$ Oversg—l1lsing Oversé+O0s¢ cosg —sing 0 
SRp=]OversA+Ising 0versg 十 cosg Overs¢—Osd |=]sind cosg 0] (3-27) 

Overs$é—Osind Overs$g TOsing lversd+cos¢ 0 0 iL 

该 变换 矩阵 等 效 于 绕 式 (2-20) 中 全 局 坐标 系 中 Z 轴 的 旋转 变换 矩阵 。 

例 3-2 绕 被 旋转 局 部 轴 的 旋转 。 

如 果 刚 体 〈 局 部 ) 坐标 系 B(COzyz) 绕 全 局 坐标 系 Z 轴 旋 转 q 度 ， 则 局 部 坐标 系 x 轴 

将 沿 着 wz 。 
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cosg —sing 01 人 coso 





ur="Rz.gi=|sing cosg 0]/0|=| sing (3-28) 
0 0 1/\0 0 
Æ u,—=(coso)I+(sing) J 的 旋转 角 9 由 罗 德 里 格 斯 公式 (3-5) 所 定义 。 
cos’ pversO 十 cosO cosgsingversd singsind 
SR，.0 一 | cosgsing vers sin2pversO 十 cosO —cosgsind (3-29) 
— singsin@ cosgsind cos0 





现在 ， 绕 全 局 坐标 系 Z 轴 旋 转 o 角 ， 接 着 绕 着 局 部 坐标 系 x 轴 旋 转 0 和 角 ， 则 变换 矩阵 
由 式 (3-30) 确定 : 
SRB =°Ru,.0°RZ.¢ 


cose 一 cosOsinp — sin@sing 
二 | sing cosOcosp ”一 cospSinO0 
0 sind cos0 
(3-30) 
PAER eH FTG (Ar wAcg) 1 = 
AT Aide 





fil 3-3 轴 和 旋转 角 。 
考虑 固定 点 位 于 A 点 的 立体 刚体 和 楼 边 
的 单位 长 度 如 图 3-2 所 示 。 



































如 果 绕 矢量 w 转动 立方 体 45°, 
& 一 (1] 1 DT (3-31) 
利用 罗 德 里 格 斯 矩阵 可 以 求 得 其 顶点 全 3-2 Ble Ri A st Boar RT 
局 坐标 : 
0.57735 
pia eo 0.57735 (3-32) 
0.57735 
0. 80474 —0. 31062 0. 50588 
Rug =Icosg + au! versé +using = | 0. 50588 0. 80474 —0. 31062 (3-33) 
—0. 31062 0. 50588 0. 80474 
顶点 的 局 部 坐标 为 
1 1 0 0 1 
BrB 一 |0|,Brc 王 |11,8rDp 王 |1|,8rE 一 |0|,BrF 一 |0 we ven (3-34) 
0 0 0 1 1 
因此 ， 利 用 sr =CRePr ， 旋 转 之 后 其 顶点 的 全 局 坐标 为 
0. 804 0. 495 证 | 0. 505 
SrB 一 |0.505 |, Srco=]1.31 |，Srp 王 |0.804 |, SrE 一 | 一 0. 31 
=] 0. 196 0. 505 0. 804 
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1.310 1 0. 196 
crF 一 |0.196|， Gre=|1|, Sry =] 0. 495 (3-35) 
0.495 1 1.31 


点 G 位 于 旋转 轴 上 ， 因 此 它 的 坐标 将 保持 不 变 。 点 B、D、 下 MH 在 一 个 由 wu 所 展示 
的 对 称 平面 内 ， 因 此 它们 将 在 一 个 圆 上 移动 。 为 了 验证 这 个 事实 ， 可 以 找 BH 或 者 FG 的 中 
点 ， 观 察 它 是 否 位 于 v 轴 上 。 将 立方 体 的 中 点 定义 为 P， 则 有 





0.5 
Pep =O ret rn) => rr) = 0.5 (3-36) 
0.5 
0.5 
6rp 一 于 (Garp 十 cr 四) 一 本 (Grr 十 erp) 一 0. 5 (3-37) 
0.5 
例 3-4 旋转 矩阵 的 轴 和 旋转 角 。 
刚体 坐标 系 B 相对 于 全 局 坐标 系 G 进行 3 个 欧 拉 旋转 角 (p、0、g) = (30°, 45°, 60°). 
将 刚体 坐标 系 B 的 坐标 转换 至 全 局 坐标 系 G 中 坐标 的 旋转 矩阵 为 
0. 12683 —0. 92678 0. 35355 
CRe= R6= (Re Ra Rz.) =R] RI oR zy =| 0. 78033 一 0.12683 —0. 61237 
0.61237 0.35355 0.70711 














(3-38) 
通过 式 (3-9) 和 式 (3-10) 可 以 求 得 这 个 旋转 矩阵 的 唯一 旋转 角 轴 。 
b=arceos 5 arC Rm —1) )=arecos—6. 14645) =98° (3-39) 
0. 0 —0. 86285 —0. 13082 
u = >— (FRza—"R}) =| 0. 86285 0.0 —0. 48822 (3-40) 
Zsing 
0. 13082 0. 48822 0.0 
0. 48822 
ù =| 一 0. 13082 (3-41) 
0. 86285 





作为 一 个 双 检 查 ， 我 们 可 以 验证 角 轴 旋转 公式 和 得 出 相同 的 旋转 和 矩阵 。 

0. 12682 —0. 92677 0. 35354 
CRB=R,,y =Icosg Húú" versg +using =| 0. 78032 —0. 12683 —0. 61237] (3-42) 

0. 61236 0. 35355 0. 70709 

例 3-5 旋转 角 轴 的 不 唯一 性 。 

旋转 角 轴 的 表述 并 不 唯一 。 旋 转 (COL u) 等 于 旋转 (一 0、 一 w) 和 “(90 十 2x，)。 

KI 3-6 旋转 矩阵 的 斜 对 称 特性 。 

对 旋转 矩阵 的 正 交 条 件 式 (2-197) 的 微分 























di a fet 
q CRB Re) = RE Rs FER BOR B= 0 (3-43) 
则 有 
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第 3 章 ”定向 运动 学 
(GREGRB)T 一 一 GREGRB (3-44) 
IÈ (3-44) 表明 (SRESRB) 是 一 个 斜 对 称 和 矩阵 。 
如 果 旋 转 和 矩阵 用 其 元 素 表 示 ,GRas 一 (ri )， 则 SRB=(r;j)。 
Gil 3-7 ”刚体 的 角速度 矢量 。 
如 果 斜 对 称 矩 阵 由 式 (3-45) 表示 
© ="REGR, (3-45) 
这 时 ， 由 旋转 矩阵 对 时 间 的 导数 可 求 得 下 列 方程 : 
GRp=°REG (3-46) 
KH. @ 是 局 部 坐标 系 B(Ozyz) 相对 于 全 局 坐标 系 G(OXYZ) 的 角速度 矢量 ，@ 是 @ 的 


ALT Pa FELD 
KB 3-8 对 于 某 一 参数 的 旋转 矩阵 微分 〈 导 数 ) 。 


假设 一 个 旋转 矩阵 R 是 变量 r 的 函数 ; 则 有 RSR CG). DTR RHEE r 的 微分 ， 








我 们 使 用 矩阵 的 正 交 特性 








RRT=I 
SQ (3-47) 两 边 分 别 对 时 间 求 导数 ， 有 
Oe act a 
dr dr 
xt (3-48) 可 重新 写成 为 
set + (Ext ] =0 





st (3-49) am| Tr) 个 斜 对 称 矩 阵 。 


KG 3-9 ”旋转 变 和 矩阵 5Rs 的 特征 值 和 特征 矢量 。 
考虑 一 个 旋转 矩阵 Rs， 绕 飞 轴 旋 转 并 不 改变 其 方向 ， 即 有 


CRea=dAu 





因此 ， 上 述 变换 式 意味 着 : 
|°Rp—AI| =0 
式 (3-51) 行列 式 的 特征 方程 为 
一 A3 十 tr(GRBe)A? —tr(FRp)A+1=0 
因 式 分 解 式 (3-52) 左边 部 分 ， 则 有 
(一 1)(12 一 人 LtrCSRB) 一 1 二 1) 一 0 


这 说 明王 1 总 是 旋转 和 矩阵 "Ra 的 一 个 特征 值 。 因 此 ， 总 存在 着 一 个 实 矢 
矢量 mi 一 忆 所 表示 线 上 的 每 一 点 通过 旋转 变换 Ra 总 保持 固定 和 不 变 。 
旋转 角 $ 由 式 (3-54) 定义 : 








1 、 1 
cosg = FLtr( Rs)—1]= 3 (rit tr22+r33—1) 


rl 





(3-47) 


(3-48) 


(3-49) 


(3-50) 


(3-51) 


(3-52) 
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其 余 的 特征 值 是 : 
A2=e'* =cos¢ +ising (3-55) 
A3;=e '*@=cosd¢—ising (3-56) 
它们 的 相应 特征 矢量 是 vw 和 w ， 这 里 v 是 v WEMAN. AWSR, 是 正 交 的 ， 因 此 mi、 
v 和 v 也 是 正 交 的 。 特 征 矢 量 v 和 vw 穿越 垂直 于 旋转 轴 n1 的 平面 。 利 用 下 列 矢 量 可 求 得 该 平 

















1 _ 
ne=>|9 +v | (3-57) 
ns= |v —v | (3-58) 
2 
FE nz 和 ns PRIT EFR IE PEFEA N : 
1 — I 4 3 — _ 
Rp na= lå: v +A3 v [=a ere +e ity | =v cos$ +v sing (3-59) 
; ] = 1 “pe = 
CRpn3= : |à2 v —A3 v j= [ety 一 e "y | =—vcosdtv sing (3-60) 














PAH, Tee FEES Ry 的 效果 是 旋转 位 于 由 通过 绕 ”1 旋转 6 fh hn Mns RP H A IR 
并 且 沿 着 1 的 矢量 是 不 变 的 。 
KG 3-10 最终 旋转 公式 。 
将 刚体 的 任何 旋转 可 应 用 于 固定 坐标 系 G 内 的 假设 是 : 坐标 系 B 和 坐标 系 G 在 旋转 之 
前 应 该 是 重合 的 。 可 以 想象 一 种 坐标 系 B 和 坐标 系 G 不 重合 的 情况 ， 我 们 想 绕 全 局 固定 轴 
Gu 旋转 坐标 系 B. 

考虑 一 全 局 坐标 系 G 和 刚体 坐标 系 Bu ， 它 们 并 不 重合 。 假 设 刚 体 坐 标 系 从 位 于 刚体 坐 
标 系 Bo 内 的 当前 位 置 绕 轴 Suw 转动 $ 角 。 注 意 旋转 轴 总 是 全 局 坐标 系 G 内 的 一 个 固定 轴 ， 
这 时 为 了 简化 可 以 去 掉 6z 的 上 标 G. 








p 





[fini 
po 




















ùú="ù =u] +u] +u; K (3-61) 
Jur tutus =1 (3-62) 


我 们 可 以 认为 刚体 B 已 经 到 达 位 置 Bo， 从 与 全 局 坐标 系 G 重合 的 位 置 开 始 ， 绕 =。 轴 
旋转 a 角 ， 接 着 绕 xo WIER p 角 ， 然 后 再 绕 zo 轴 旋 转 y 角 。 

SBA Bo 位 置 重合 的 刚体 坐标 系 B。 当 我 们 将 局 部 坐标 系 先 后 绕 = 轴 旋 转 y 角 ， 绕 y 
轴 旋 转 2 角 时 ， 局 部 > 轴 将 与 旋转 轴 Sw 重合 。 想 象 一 下 刚体 坐标 系 B， 让 我 们 通过 Bi 说 明 
它 。 这 时 我 们 绕 > 轴 Si) 旋转 $ 角 ， 再 进行 一 系列 反 向 旋转 ， 即 绕 y 轴 旋 转 一 0 角 ， 
绕 = 轴 旋 转 一 p 角 。 这 种 操作 的 结果 等 效 为 : 从 Bo 开始， 将 刚体 坐标 系 B 绕 之 轴 旋 转 


























$ ffi. 
因此 ， 刚 体 的 初始 相对 定向 必须 已 知 ，Bo。 和 G ERMEER 是 一 个 已 知 的 矩阵 。 
bu biz bi 
GRo=[b;]=| bn bz? bz (3-63) 
b31 b32 bss 


已 知 变换 矩阵 SR。， 我 们 便 可 以 求 得 角 a、B、Y。 
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cacy—cBsasy cysatcacBsy sBsy 


GRo=R-z,.yRao.pRey.a =| — casy —cpcysa cacBcy—sasy cysf (3-64) 
sa sf — cash cp 
b31 
a = —arctan 一 一 (3-65) 
b32 
B=arccosb 33 (3-66) 
bis 
7 一 arctan —— (3-67) 
b2 





罗 德 里 格 斯 公式 可 确定 Bo 和 B 之 间 变 换 矩 阵 。 然 而 使 用 罗 德 里 格 斯 公式 ， 必 须 将 GZ 
表示 在 Bo 之 中 。 





On =GCRIGY=GRIu (3-68) 
°Rp =Icosg Hù’ ùT vers¢ +° using 


=Icos$ + (SRIu) (CRT ú)Tversg +2Rg uoRo sing 





=Icos¢ +@R oun? Ro vers +OR i uoRo sing (3-69) 
刚体 坐标 系 与 全 局 坐标 系 最 终 位 置 之 间 的 变换 矩阵 5Ra 为 
CR», =°Ro° Rpg 
=C Ro [Icosg ERT uu TeRoversg #ERTUCRo sing | 
=C Ro cosg +| úúT ]JSRoversg +uSRosing 
一 "有 Ra. CRo (3-70) 
我 们 将 式 (3-70) 称 为 最 终 旋转 公式 。 从 具有 变换 矩阵 “Ru HBRAG 的 位 置 开 始 ， 刚 
体 坐 标 系 BSH =c n 旋转 8 角 之 后 ， 它 决定 刚体 坐标 系 了 B 与 全 局 坐标 系 G 之 间 的 变换 
矩阵。 
作为 一 例子 ， 考 虑 一 个 绕 Z 轴 旋 转 30 的 刚体 ， 该 刚体 在 Bo 处 。 




















eo a 
mee eae 0.866 一 0.5 0 
GRo=| . x x ~| 0.5 0.866 0 (3-71) 
sin 一 cos 一 0 
6 6 0 0 1 
0 0 1 














— Gis] (3-72) 
2 
因此 ， 

1 0 0 0 0 0 1 0 0 0.866 —0.5 0 
GR, =|lo 1 0 cos T 6 0 =i GRocos 了 十 0 0 0 1— cos $ ) 0.5 0.866 0 
gog l 0 1 0 0 0 0 0 0 1 

0.866 —0.5 0 
=| 0 0 =l (3-73) 

0.5 0.866 0 
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在 ar 二 27 处 的 刚体 点 将 被 转换 至 全 局 坐标 系 中 的 矢量 Sr : 
0.866 一 0.5 0 \(2 1. 732 
Gr =GRpBr =| 0 0 —1//o/=| 0 (3-74) 
0.5 0.866 0 八 0 1 
KG 3-11 被 转 刚体 的 旋转 。 
一 刚体 绕 Y 轴 已 经 旋转 30"。 我 们 需要 将 该 刚体 绕 飞 旋转 45° 
Lee Var 12 








1 =F y= 上 一 +—K (3-75) 
V3 /3 /3 
由 于 第 1 次 旋转 30"， 因 此 有 
oe og 0.866 0 0.5 
GR = 0 1 0 |=| 0 1 0 (3-76) 
x x —0.5 0 0.866 
一 Sn 一 0 cos— 


6 6 
使 用 最 终 旋 转 公 式 式 〈3-70) ， 能 够 确定 所 要 求 的 旋转 变换 矩阵 为 
0. 80474 一 0.31062 0. 50588 
GR, = Icos +Lau! Jversg using =| 0.50588 0. 80474 —0. 31062 | (3-77) 
—0. 31062 0. 50588 0. 80474 
0.44399 一 0. 31062 0. 84047 
SRB=°R;,s Ro= 0.59341 0. 80474 —1. 6065X107? (3-78) 
=0. 67137 0. 50588 0. 54162 





3.2 欧 拉 参数 


假定 p 是 局 部 坐标 系 B(COzyz) 相对 于 全 局 坐标 系 G(OXYZ) Ba=uil+u.J 十 usK 
轴 的 旋转 角 。 旋 转轴 的 存在 是 一 个 绕 刚 体 旋转 的 欧 拉 定理 的 分 析 表 述 : 具有 一 固定 点 刚体 的 
最 一 般 的 位 移 是 绕 某 轴 的 一 个 旋转 。 

为 了 求 得 旋转 轴 和 旋转 角 ， 我 们 引入 欧 拉 参数 eo 、el ez, es, 


e1、e2、e3 是 矢量 e 的 分 量 。 

















CHR eo 是 一 个 标量 ， 


Wi 








bee (3-79) 
2 
e eri tez] Hes R =ùsin É (3-80) 
ei +e, +e +e =e eT.e =1 (3-81) 
这 时 ， 利 用 欧 拉 参数 可 推导 出 能 满足 方程 Sr =S Rer 的 变换 矩阵 SR 为 
GRp=Ri,s= (e? e?)I+2e .eT2e0e 

eg teie, e, 2(eies—eoes) 2(eoesteies) 

=| 2 (egez Fejes) e —et te — e, 2 (€2€3 ege) 

2(ele3 一 eoe2) 2(eoel 十 eze3 ) egei ey te; 
(3-82) 
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AF, e 是 与 e 相应 的 和 斜 对 称 矩 阵 ，s 的 定义 如 下 : 


0 ies es» 
e=| e; 0 =e] 
=e, el 0 





给 定 一 个 变换 和 矩阵 SRp， 可 以 由 式 (3-84) 获得 欧 拉 参 数 . 
1 、 
eo 一 本 (tr(SRe) 十 1) 


1 
4e, 


通过 式 (3-86) 及 式 (3-87) 确定 旋转 角 p MEE HH ù: 





(GRp—SRE) 


a 
| 





1 : 
cosg =z ltrCRs)— 1) 


(CRg—ER}) 





4 2sing 
对 于 任意 的 较 大 旋转 ， 欧 拉 人 参数 提供 一 个 非常 合适 的 、 宛 余 的 、 
证 明 : 
3-3 描述 了 一 个 具有 位 置 矢量 r 的 刚体 点 了 ， 
以 及 沿 着 固 于 定 全 局 坐标 系 中 旋转 轴 ON 的 单位 矢 
Eú, Su 轴 旋 转 风 角 之 后 ， 点 移动 到 忆 - 且 其 位 置 
矢量 为 r'。 为 了 求 得 位 置 撩 量 r MAER Er Z E W 
关系 ， 我 们 通过 式 (3-88) 表达 7 : 
r=ON+NQ+QP” (3-88) 
通过 分 析 图 3-3， 我 们 可 以 利用 r 、r 、 立 和 gY 描 o) 
述 式 (3-88). 
r'=(r UtiX(r Xûů)cosg—(r Xi)sing 
=(r ¢a)at+lr —(r + nlcosgt (Xr)sing 
(3-89) 
调整 式 (3-89) 可 得 到 罗 德 里 格 斯 旋转 公式 的 
新 形式 为 





























Al 3-3 














r'=r cos¢+(1—cos¢) (a * Nata Xr)sing 
利用 式 (3-79) 和 式 (3-80) 中 的 欧 拉 参 数 ， 基 于 下 列 三 角 关 系 
$ 


cosg =2cos? 二 一 1 


2 





sing =2sin $cos > 

1—cos¢ = 2sin* 2 

将 罗 德 里 格 斯 公式 3-90) 转换 成 一 个 更 有 用 的 形式 : 
r'=r(2e5—1)+2e(e * r)+2e0 (eXr) 

使 用 一 种 更 为 简便 的 形式 ， 为 r 一 Sr 和 r =r 定义 新 的 注解 。 





Y 
旋转 轴 和 旋转 




















(3-83) 


(3-84) 


(3-85) 


(3-86) 


(3-87) 


非 齐 次 旋转 描述 。 





(3-90) 


(3-91) 


(3-92) 


(3-93) 


(3-94) 
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Gr 一 (ez 一 ez )Brt+2e(eTBr) +2e, (e8r) (3-95) 
从 式 (3-94) 右边 提出 位 置 矢量 sr 公 因 子 ， 便 可 得 出 欧 拉 参数 变换 矩阵 SR : 
GRg= (ef —e’)I+2eeT + 2e č (3-96) 
这 里 ， 
Sr 一 5RBBr =Ri,y tr (3-97) 


对 于 给 定 变 换 敌阵 Re， 为 了 说 明 旋 转角 p 和 旋转 轴 & 的 方程 ， 我 们 计算 “Ra 的 迹 以 便 
求 得 ep 和 $: 





tr(CGRB) 一 4e7 —1=2cos¢ +1 (3-98) 
计算 GRs 一 CRE 以 求 得 e Ala. 
0 —4e es 4e ye, 0 = this Uy 
GRp— SRE= de ye, 0 —4e,e, =2sing Us 0 ul (3-99) 
一 4euey evoel 0 —u, Uy 0 
Ta 193 
1 
e 一 一 一 r 3 31 (3-100) 
4e 
Toi Tip 
Fag Vo 
4 1 
u= psina 713 t31 (3-101) 
ro Tiz 
证 毕 。 


KG 3-12 ”变换 矩阵 “Ra 的 轴 角 旋转 。 
ehh a= H] HR) N3 旋转 30?" 的 欧 拉 参数 为 


o 








0 
e) = cos 7 =0. 966 (3-102) 
a TOE oe 
e —usin -7 =e,I+e,J +e,K=0. 1491 +J +K) (3-103) 
因此 ， 相 应 的 变换 矩阵 5RB 为 
0. 91 一 0. 244 0. 333 
C Rg = 0: 333 0.91 — 0. 244 (3-104) 


—0. 244 0. 333 0. 91 
KB 3-13 ” 欧 拉 参数 和 欧 拉 角 的 关系 。 
比较 式 (2-106) 中 的 欧 拉 角 旋转 矩阵 和 式 (3-82) 中 的 欧 拉 参数 变换 矩阵 ， 我 们 可 求 
得 欧 拉 角 和 欧 拉 参数 之 间 的 关系 : 











0 gtg 
€o = cos COS 7 (3-105) 
. 0 4? 
ei sin 3 cos 2 (3-106) 
. 0 .一 9 
e,=sin > sin 7 (3-107) 
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90 , ote 
e, = cos > sin 7 (3-108) 
或 者 ， 

2(e,es teoel) 
p =arccos mg —— (3-109) 
0=arccos[2(e? +e3)—1] (3-110) 

—2(e,e,—e,e, ) 
:)=arccos n (3-111) 








sing 
KG 3-14 旋转 角 p= kn 的 旋转 矩阵。 
当 旋 转角 为 $= 二 kx,， R=H1, 2, 3, sei, ec, =0. Al, XR (3-82) 的 欧 拉 参数 变换 
和 矩阵 变 为 





1 
any C1 es erez 
GP 一 2_ 1 
Rp=2| eje, ey eez (3-112) 
yl 
€1&3 CHE 63 g 





SR 是 对 称 和 矩阵 ， 这 说 明 B= ko 和 $$ 二 一 kr 是 等 效 的 。 
KB 3-15 ”微分 旋转 矢量 。 
对 于 微分 旋转 矢量 角 dg ， 可 以 考虑 罗 德 里 格 斯 公式 ， 




















7 一 r 十 (全 Xr)dg (3-113) 
在 这 种 情况 下 ，r 和 r 之 差 也 是 非常 微小 的 ， 
dr 一 r 一 r 一 dg2 Xr (3-114) 





因此 ， 绕 着 单位 矢量 之 所 说 明 的 轴 旋 转 的 一 个 微分 旋转 角 dy 是 沿 着 如 且 幅 度 为 dy 的 
一 个 矢量 。 式 (3-114) 两 边 都 除 以 dd WA 
r=0 Xr (3-115) 
RAHA EER SE ù 轴 旋 转 的 全 局 速度 矢量 。 
KB 3-16 旋转 矢量 的 指数 形式 。 
KERAMER Er 的 全 局 坐标 系 B 上 的 一 个 点 PP。 如 果 刚 体 有 一 个 角速度 w ， 则 点 P 
在 全 局 坐标 系 中 的 速度 为 














r =o Xr 一 wr (3-116) 
这 是 一 阶 线 性 微分 方程 ， 它 可 被 积分 为 
r (1)=e®'r (0) (3-117) 


这 里 ，r (0) 是 点 P 的 初始 位 置 矢 量 ，e”: 是 一 个 矩阵 指数 。 


(at)? (OO1)3 








eco 一 I 十 oz a Sy pee (3-118) 

角速度 @ 有 一 个 幅 值 w MARE ú 所 说 明 的 方向 。 因 此 ， 
四 = wt (3-119) 
O =wu (3-120) 


WS 
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ot=wtu =u (3-121) 
因此 ， 
a E $$ ~ 
ot —,¢u — J4 iu | aai 2 = 
e e In ( 31 | 51 Ja | B AN | 6 u (3-122) 
ett =]+ using +u? (1— cosg) (3-123) 




















这 是 一 种 罗 德 里 格 斯 公式 的 可 替代 形式 ， 式 (3-123) 表明 et ERMER Er =r (0) 
变换 至 全 局 位 置 矢 量 cr SrO) 的 旋转 变换 。 
KB 3-17 ee 红 的 旋转 特征 。 
为 了 说 明 erT ES, S 是 旋转 矩阵 的 集合 
S 一 (及 ER3<3 :RRT 一 工 | 及 | 一 1) (3-124) 
我 们 必须 要 说 明 R= 二 e 吉 具有 正 交 特性 ， 即 RTR 二 I， 以 及 它 的 行列 式 是 |R | 一 1。 通过 
式 (3-125) 可 验证 etë WEZE. 
(et# )~l=e—$¥ =e o4T = (e$t )T (3-125) 
X, Ro'=R7™, AUK RRT=1, PIE ZEEE |R| = +1; 由 指数 函数 的 连续 性 ， 可 得 
le? |=1. Ak, |R|=1. 
KG 3-18 et" SF he $e FE MES Re. 
扩展 式 (3-126), 





















































e*t =]+ using +u’ (1— cosg) (3-126) 
可 得 
ui vers$ +c¢ UU, VErS — U, S uiusversg TU,Sh 
ef" =| u u, versg Hu, so u;vers¢ +c U, U, Vers — u] SP (3-127) 
U,UzVErSP—UySP UyU,VvErsd TU, Sh u? vers tc 
该 式 等 效 于 轴 角 方程 式 (3-5)， 因 此 有 
ef” =—=Ra,s =" Rp=Icosg Húú versg Husing (3-128) 


KB 3-19 罗 德 里 格 斯 旋转 公式 的 新 形式 。 
考虑 图 3-4， 可 以 写 出 : 


























cos É [MB | =sin $ | NM | (3-129) 
|INM|IMP’= |MP’ |i XNM (3-130) 
求 得 
[cos £ MP" =(sin jax NM (3-131) 
现在 使 用 下 列 等 式 
2 MP’=MP’—NP (3-132) 
2 NM=NP’+NP (3-133) 
NP'—NP=r'—r (3-134) Y 
aX (NP’+NP) =a (r' +r) (3-135) 图 3-4 ， 例 3-19 参考 图 
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能 够 写 出 罗 德 里 格 斯 旋转 公式 的 男 一 种 形式 : 
cos 号 Cr' 一 门 一 sin f X(r'+r) 
或 者 
Gr) = tan Sax Gr +r) 
KG 3-20 罗 德 里 格 斯 矢量 。 


w 一 tan >ù 



































2 
(3-138) 的 矢量 被 称 为 罗 德 里 格 斯 矢量 。 依 据 式 (3-139)， 我 们 可 以 知道 
与 这 个 参数 相关 。 
e 
y= 
eg 
1 1 
eo 5 
1+wiw 1+w? 
w, w, 
ë= = : i=1,2,3 
lt+wl—w i+w’ 





(3-136) 


(3-137) 


(3-138) 


欧 拉 参 数 


(3-139) 


(3-140) 


(3-141) 


式 (3-82) 中 ， 罗 德里 格 斯 公式 可 以 被 转换 成 一 种 基于 罗 德 里 格 斯 矢量 的 新 形式 。 





1 
1+w'w 











GRp=Ra.s = (e? e’)I+2ee'4 2e,e= 


两 种 旋转 w 和 w 的 合成 等 效 于 单一 旋转 w ， 即 
_w"t+w ‘yw’ xy / 
1—w T, w 7 





w 


KB 3-21 旋转 矩阵 SRa 的 元 素 。 
引入 勒 维 奇 维 塔 (Levi-Civita) 密度 或 者 排列 符号 : 
1 ijk=123=231=312 
Ep = 0 i=j, j=k,k =i 
1 ijk=321=213=132 











1 
E ijk zC DG k)(k i) 1;7 sk =1,2,3 


回忆 式 (2-201) 中 的 克 罗 内 克 6 RZ: 
0 =1G=j); 6;=0(i 区 ]) 
可 通过 式 (3-147) 重新 定义 罗 德 里 格 斯 旋转 矩阵 中 的 元 素 。 
r; =0; cosp + (1—cos¢ )u,u,; Te ,,u, sing 


3.3 欧 拉 参数 的 确定 


假设 一 变换 矩阵 已 知 。 


Ca—wT—w )I+ 2w'w +2w | 


(3-142) 


(3-143) 


(3-144) 


(3-145) 


(3-146) 


(3-147) 
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ti Tiz Ti 
5RB 一 Voy Moo T23 (3-148) 
P31 32 Tig 


这 时 ， 有 可 能 求 得 欧 拉 参 数 e。、e; 、e，。、e;， 也 有 可 能 利用 下 列 4 组 方程 中 的 一 个 方程 


3 















































说 明 旋 转轴 和 旋转 角 : 
| 1 | | | 
i 2 lr Tr Pa, 
(3-149) 
_ 1” "z ht ie "a ee Se "r 
si 4 ey e2 4 eo “3 4 eo 
| | | 
e= E 7 lrir 71g. T33 
(3-150) 
_ 1ratr 1ratrns _ 1732 Tr» 
S 4 el °3 4 e1 €o 4 el 
1 
C= > lrir t ras Ti 
(3-151) 
1ra trz _ 1 Tig Ts 17r21 十 ra 
“34 es» “Oo 4 ey | es 
| l | 
e, =E 2 1-111, Taa T1393 
a 有 (3-152) 
-Ira rge lra Ti _ 1732 Fr» 
€o 4 es “1 4 es “2 4 es 





虽然 式 (3-149) ~sh (3-152) 代表 了 4 个 不 同 的 解 集 ， 但 是 它们 所 计算 的 欧 拉 参数 e,、 
eis es, e, 是 相同 的 。 因 此 ， 通 过 使 用 带 有 最 大 公约 数 的 集合 ， 数 值 的 误差 可 以 被 最 小 化 。 

加 减 符号 说 明 绕 着 之 轴 旋 转 6 AEAT a 轴 旋 转 一 $ 角 。 

iE AA : 

比较 式 (3-82) 和 式 (3-148) 的 结果 说 明 : 对 于 第 1 组 方程 式 (3-149)， 通 过 对 角 元 
R rio roy. Ta KAISE tr(CRg)=4e 一 1， 可 以 解 得 e，。。 为 了 获得 e,、e,、es， 需 要 
分 别 简化 ras — razn ria 一 ri All ro, Tigo 

从 式 (3-150) 至 式 (1-52) 的 其 他 解 集 ， 也 可 以 通过 比较 求 得 。 

KP 3-22 由 旋转 矩阵 Rs 所 得 的 欧 拉 人 参数 。 

已 知 一 变换 矩阵 : 
































0.5449 —0.5549 0.6285 
GRB 一 | 0.3111 0.8299 0.4629 (3-153) 
—0. 7785 一 0.0567 0.6249 
为 了 计算 相应 的 欧 拉 人 参数， 使 用 方程 3-149) R: 
trCSRB) 一 ~ 十 ro 十 rs 一 0.5449 十 0.8299 十 0. 6249 一 1. 9997 (3-154) 
因此 ， 有 





eu 一 VELtr(cRB) 十 1 /4 一 0.866 (3-155) 
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e,=—0.15 e,=0.406 e,=0.25 (3-156) 
Bl 3-23 ” 当 已 知 其 中 一 个 时 的 欧 拉 参数 。 
戎 虑 绕 着 由 单位 矢量 说 明 的 轴 旋 转 p 角 的 欧 拉 参数 旋转 矩阵 (3-82)。 旋 转 和 矩阵 SRB 非 
对 角 线 元 素 




















e061 一 Ga Toz) 
1 

ejes” grs rs) 
1 

e023 一 gra riz) 

(3-157) 

1 

eje; 5 1 (ri Tra 
1 

eye, = Gig Tra) 
1 

€2€3 =q (123 Traz) 





WARE AM (3-157) 中 的 一 个 方程 时 ， 式 1-57) 可 以 用 来 求解 e,,， i=0, 1, 2, 3, 
友 例 3-24 利用 斯 坦 利 方法 求解 欧 拉 参数 。 
下 面 是 由 斯 坦 利 (Stanley) 给 出 的 一 个 有 效 方法 ,我 们 可 以 首先 求 得 4 个 eF: 














一 也 [1 二 tr(GRe)] 

ef =F [142r 一 tr(GRe)] 

ef =F [i t2r,, —tr( CRs)] 

08 = TU 1+ Org —tr( Rs)] (3-158) 


取 最 大 的 e? 求 其 正平 方 根 。 这 时 基于 最 大 值 e;,， 其 他 的 e, 可 以 通过 除 以 式 (3-157) 
中 6 个 方程 中 合适 的 3 个 方程 求 得 。 




















3.4 四 元 数 
全 局 四 元 数 g 作为 一 个 量 定 义 如 下 : 
gq=go +4 =a, tqil +4, +qsK (3-159) 
EP, q 是 一 个 标量 ，g 是 一 个 矢量 。 四 元 数 也 可 以 标记 形式 展示 。 
q=qo qii tqj tgqsk (3-160) 
此 处 的 i、7、& 是 标记 ， 定 义 如 下 : 
== =e (3-161) 
ij =—ji=k (3-162) 
jk=—khj =i (3-163) 
ki=—ik=j (3-164) 
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两 个 四 元 数 相 加 仍然 是 一 个 四 元 数 ， 即 : 
qt+p =Q, tgo tp, +p) 
=go tqiitgqsj tgk tpo tpii tpj Tpk 
=(q Fp) tl Hp itU, Fp) (qs Tp,)k 
两 个 四 元 数 相 乘 仍 然 是 一 个 四 元 数 ， 定 义 如 下 : 
ap = Ta) (potp) 
=q Po Tao P F pog Hap 
=qopo T4 P tap Pog tap 
= (do po qi Pı Taba b3) HC Pogi FPido Fap; TP 392% 





























(3-165) 


| (Pode ro Po P2143 Gipsy | (pods Pid Fdo Ês rp qk (3-166) 


XE, qp 等 于 矢量 g 和 矢量 p 的 又 积 减 去 其 点 积 。 
qp—qX<p—q°p 





(3-167) 


四 元 数 加 法 具有 结合 性 和 交换 性 。 四 元 数 乘法 不 具有 交换 性 ， 然 而 它 具 有 结合 性 和 分 





配 性 。 
(pq)r= par) 
(p+qQr=pr+aqr 
四 元 数 q ASEH Mc g* 。 
一 do 一 gd 一 0 一 0 一 0 





因此 ， 有 





(3-168) 
(3-169) 





(3-170) 


gg * = (dy +) (go —D = 999 +909 Po I—499= 999) Ha ata Xgq=ge tai +43 +43 


这 上 时， 





la|=Vgg” = Jai tai ra, Fae 











如 果 qg 是 一 个 单位 四 元 数 ，|g | 二 1， 则 有 gq !==g*。 
假设 e($，u) 是 一 个 单位 四 元 数 ，|e($,u) | 二 1， 则 

$ 
| | 


e(d.u)=e, te =e, +e, Tie, J +e, K=cos>+sin >a 


2 2 








r=O+r 是 一 个 四 元 数 ， 相 当 于 一 个 纯 矢 量 r。 绕 着 旋转 8 角 后 ， 矢 量 r 将 


r=e(d.u)re* (ġ,u) 
FAF: 


Sr =e(ġ,u)Pre* ($,u) 


(3-171) 


(3-172) 


(3-173) 


(3-174) 


E 
FE: 


(3-175) 








TAG. re Fe AE RE Rg A FA AY JCE, BI e (6 ,zx ) =cos | sin 


连续 旋转 尺 二 RpR1 AL ASK e(p.u) =e, (p2.u,)e,(Pisu,) 定义 。 








注意 : e, (gis uy). e, ($2, us), … 是 四 元 数 ， 与 此 同时 e,、 Cir Corn €3 是 欧 拉 参数 。 


iE BA: 
利用 四 元 数 乘法 (3-166)， 我 们 可 以 写 出 
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re* =eyr +rXe* 一 re 
ry ry — e] 
=e | r2 [F| r2 |X e> |+ (eir; Fers Fer, ) 
rs r3 ~~ € 3 
Col, Terz 6E372 
= (eritesrstesrs) H| eof, ers Fer] (3-177) 
Cor3 Tero” €r] 
因此 ， 有 
ey Col y Tegra Ezra Cor, Tees Egi? 
ere * =e, (e,r,; te gr, Fera) Fle] egra er; Feri | 十 euleory 一 el7r3 十 esrl | 十 
e3 Cog Ter,” er] Cog Tere” er] 
ei ei Cory meg y .Col 


Celrl1 十 ezrs 十 esrs)|es | tle, |X Co, 一 elr3 十 es71 





e, ëz Cory Feira ery 
Fi 
一 “RbB|7， (3-178) 
7 3 
这 里 ,SRa 等 效 于 欧 拉 参数 变换 矩阵 (3-82), 
el +e? eset. leje — ezez) Veep ree 
SRB 王 | 2(e ,e, Fe, e,) ee, tes — e? 2(e,es—eoel) (3-179) 
2(ele3 一 eoey) 2(eyeiteses) ebe? —e, Fe, 


利用 类 似 的 方法 也 可 推出 : fr 一 e*(8，u)re($，u) 是 r 一 e (8,u)re*(8，u) 的 逆 变 


换 ， 它 等 效 于 : 


Br =e*(d.u)°re(g¢su) (3-180) 
现在 ,假设 。 (b+ u) Ale, (b+ up) 均 是 四 元 数 ， 它 们 分 别 相当 于 旋转 变换 矩阵 R，， 和 


Rs,。 第 1 次 旋转 变换 将 ar 变换 成 r ， 第 2 次 旋转 变换 将 忆 r 变换 成 外 r 。 因 此 ， 有 


Bop =e (glui) rer (dy tt) (3-181) 
Bsp =e, (p, su.) Pres ($s ,us ) (3-182) 
Bp =e, (p, ut)el(Cg ay) rey Giggles ($s,u,) (3-183) 
sk (3-81) 表明 : 
elpu) =e, (by uy de, Cpi ul) (3-184) 


elp, u) 是 四 元 数 ， 这 相当 于 R=R2R.. 

KB 3-25 利用 四 元 数 法 求 取 罗 德里 格 斯 公式 。 

化 简 式 (3-178) 可 得 与 罗 德 里 格 斯 公式 相 类 似 的 矢量 形式 。 
r'=e(¢.u)re*($,u)=(e, —e "er +2e,(eXr)+2e (eer) (3-185) 
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太 例 3-26 ”利用 四 元 数 法 说 明 旋转 的 构成 。 
利用 四 元 数 法 描述 旋转 能 够 使 计算 旋转 构成 变 得 简单 。 如 果 四 元 数 e1($, i) 代表 着 
旋转 Rango IUTER e, (py, a) 代表 着 旋转 R;,,s, ， 那 么 两 者 的 乘积 : 
e, (by sity Je ($y sty) (3-186) 








代表 着 R;,,s,R2 ,s, ， 因 为 : 
Ri,,s, Ra gr =e, gods) Le, C ù ret (oú lef (byt) 
一 [Ley (gost er Bi) Ir Le? (6,50, es ($, ,0,)] 
= Le, (8 人 Je, 66, 90,) Ir Le, Cysd) er (6,00;)]* (3-187) 
KB 3-27 主 全 局 旋转 矩阵 。 
与 主 全 局 旋转 矩阵 Rz.。、Ry.s 和 Rx,y 相 关联 的 四 元 数 是 : 




















0 
e(a,K)=| cos = , Sin = 0 (3-188) 
2 2 
1 
0 
e(B.J)=| cos i „sin ’ 1 (3-189) 
0 
1 
e(y, D= sos arsin 0 (3-190) 
2 2 
0 














利用 式 〈3-188) 一 式 (3-190)， 能 够 导出 其 基本 的 变换 矩阵。 下 面 我 们 来 求 Rz,。。 





Rz.s—=° Rp= (e? —E *)I+2EET 4 2,8 = (cos Z sin? a +2 KKT+2cos “sin SK 








2 2 2 
cosa —sina 0 
= 区 cosa | (3-191) 
0 0 1 


* Gil 3-28 el, ù) 的 内 自 同 构 特性 。 
由 于 e($ ,是 一 个 单位 四 元 数 
e* (¢,u)=e 1(g,) (3-192) 





因此 ， 可 以 写 出 
Sr =e(¢,u)Pre '(d.u) (3-193) 
在 抽象 代数 学 科 中 ， 映 射 形式 r Sqr | RAAT. EA 0 Ae ae Be A 
乘 计 算 所 得 。 这 样 一 来 ,Sr 便 是 sr 基于 旋转 四 元 数 e($，w) 的 内 自 同 构 。 


六 3.5 自 旋 张 量 与 旋转 张 量 


有 限 的 旋转 可 用 两 种 常见 方式 来 表达 : 第 1 种 方式 使 用 被 称 为 旋转 量 的 3X3 KIEZE 
阵 尺 ， 它 是 旋转 张 量 的 缩写 ; 第 2 种 方式 使 用 被 称 为 自 旋 的 3X3 KRATER ú, C A 
旋 张 量 的 缩写 。 

一 个 旋转 就 是 一 个 线性 操作 ， 当 旋转 轴 w 和 旋转 角 Y 已 知 时 它 将 3r 变换 至 Sr 。 
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Sr 一 5RBBr (3-194) 
Aléu HAFRE ù, PA a 和 $$ 描述 一 个 旋转 。 
GRp= (Icos¢ +uuTvers$ +using ) (3-195) 


Flat. MERMER |ú | 二 Yui 十 us 十 us 二 1， 探 索 用 于 说 明 旋 转轴 的 非 单位 矢量 。 通 过 
正 接 相 乘 可 计算 出 的 平方 。 




















Us us Ujug Uus 
7 2 2 a robes aa 2 
u?=| uiu, m U: uus |=— uu" =— u u=úú"—u“I (3-196) 
一 
UUus UUus uUi Us 








这 是 一 个 具有 迹 tr@’)=—2|a|?=—2u?= 2u? tus us :) 的 对 称 和 矩阵 ， 它 的 特征 
值 分 别 是 0、 一 w” 和 一 w? 。 换 句 话说,，& 满足 它 自己 的 特征 方程 : 
CH=, = ,=u n>3 (3-197) 
HAA MH ai eR IE EL a FR EAST PY. SC TR eS, RAER PS IEE AY EK 
Fe ee OT PR HY 
自 旋 与 旋转 是 互 为 函数 ， 因 此 R AA a AE (Taylor) 级 数 展开 : 
R=I+Tc Ud 二 +c, 二 us 十 … (3-198) 
然而 ， 由 于 可 以 去 除 式 〈3-197) PS wa, AR AWE I, a. u’ 的 线性 



































br? Cu? | us) H1 aku, +br*u,u, aku, +ba* u, u; 











R=I+aQu)+bQua)?=| adu, FOÀ?’ U, us bà? u? | us) H1 adu, +ba* u us 
—adu, FDA?’ uu, aàu, FOA’ Usu; bà? u? | uz) wl 
(3-199) 


这 里 , A Ejmi ARE A. Ikt, a 一 a($, u) Mb=bC, u) 是 旋 
转角 和 天 的 一 个 不 变量 的 标量 函数 。 
回顾 u= Ju tus tus =1, 旋转 张 量 R EH Ra., b 以 及 自 旋 量 X& 的 一 个 函数 ， 表 


3-1 收集 了 一 些 有 关 R 的 描述 。 
表 3-1 旋转 张 量 R 作为 系数 及 自 旋 量 的 函数 



























































a b À R 
sing sin -5 1 I+ singu +2sin zu 
I+ 2cos? $ (tan $ u+ tan? $ a’) 
$ 2 2 2 
2cos 2 2cos 2 tan -了 7 g ERES 
(I+ tan 2 n) (1 tan 2 a 
2cos £ 2 sin = I+ 2cos É sin i u + 2sin? Í m’ 
R 2, 2 2 en 2.$ -2 
F sing p 2 $ I+singu +2sin zu 
证 明 : 


假设 4 二 1， 由 于 式 〈3-199)， 可 以 求 得 和 矩阵 的 迹 trR ) 王 1 十 2cosy 等 于 
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tr( 及 ) 一 1 十 2cosg 一 3 一 20x2 
因此 ， 有 
1 一 cosg 2 ., $ 


2 2 
ZL ZL 2 


b= 





现在 ， 正 交 性 条 件 











I =RTR =a +a”) (ta +607) =14 (260—a Da +b a 


=[+(2b—-a?—b?u*) a 
由 此 求 得 





> 1 
a=V2b—b?u* =—sing 
因此 ， 
$> $ se J 


| NE 2 aw ~2 
R=I+ —singu + — sin ou =I+singu + vers¢u* 
u u 2 








2 


(3-200) 


(3-201) 


(3-202) 


(3-203) 


(3-204) 


从 数字 化 角度 ， 对 于 较 小 的 $ 值 ， 正 弦 平 方 的 形式 受到 青睐 以 避免 在 计算 1 一 cosg 时 的 


相互 抵消 。 取 代 式 (3-199) 中 的 系数 w、0， 可 得 有 关 云 和 y 的 明确 旋转 张 量 。 


u? (us +u? cd 2u th, 8? f 


R =| 2u,u,s? tussy hue us ep 2u,uss? 





这 等 效 于 式 〈3-13) 。 
WR AA 但 为 非 零 ， 则 有 a 二 二 sing All b = 


KB 3-29 自 旋 张 量 的 特征 值 。 
考虑 由 下 列 矢量 所 说 明 的 旋转 轴 。 





2 
Qu co T 





相关 联 的 自 旋 矩阵 及 其 平方 为 








18 6 40 














a 的 特征 值 是 CO. 7i, —7i). TARY a? 的 特征 值 是 (0， 一 49， 一 49) 。 


六 3.6 表述 旋转 过 程 中 的 问题 


u,sp 2ulus,s’ +ussy 
$ 
g wsi 


2uiuss? É— usp 2u,u;s? 22 Tu is¢ u? +u? tus red 


(3-205) 


， 它 们 并 不 影响 R 的 取 值 。 


(3-206) 


(3-207) 


(3-208) 


显而易见 ， 在 本 章 中 介绍 了 很 多 不 同 的 方法 用 于 描述 旋转 ， 然 而 它们 中 只 有 一 部 分 有 着 
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本 质 的 不 同 。 相 对 于 一 些 参 考 坐 标 系 ， 完 全 描述 刚体 定位 的 参数 或 者 坐标 有 时 被 称 为 姿态 坐 
标 。 相 对 于 无 可 争议 的 旋转 特性 ， 在 描述 旋转 中 有 两 个 内 在 的 问题 。 

1) 旋转 不 能 交换 。 

2) 空间 旋转 并 不 拓扑 地 允许 在 三 维 欧 氏 空间 中 存在 一 个 平滑 映射 。 

在 前 面 的 章节 中 ， 本 书 讲述 了 旋转 的 非 互 换 性 。 不 管 怎样 ， 遵 守 旋 转 次 序 是 非常 重要 
的 ， 尽管 有 时 看 起 来 我 们 可 以 改变 旋转 次 序 并 且 能 够 获得 相同 的 结 

在 三 维 欧 氏 空间 中 缺乏 光滑 映射 ， 这 意味 着 我 们 并 不 能 顺利 地 利用 一 组 3 个 数 捅 述 每 种 
旋转 。 任 何 组 中 的 3 个 旋转 坐标 包含 至 少 一 个 几何 定位 ， 在 几何 定位 中 坐标 是 单数 ， 这 意味 
着 至 少 两 个 坐标 并 没有 被 定义 或 者 不 唯一 。 这 个 问题 类 似 于 定义 一 个 坐标 系统 以 定位 地 球 表 
面 上 的 一 点 。 使 用 经 度 和 纬度 在 南北 极 处 就 变 成 了 问题 ， 在 南北 极 处 ， 一 个 微小 的 位 移 在 经 
度 上 就 能 够 产生 一 个 径 向 变化 。 我 们 不 能 找到 一 个 超级 系统 ， 因 为 不 可 能 用 一 个 平面 平滑 地 
包 囊 住 一 个 球体 。 类 似 地 ， 不 可 能 顺利 地 用 三 维 欧 氏 空间 包 夺 住 空间 旋转 的 空间 。 

这 就 是 为 什么 我 们 有 时 用 4 个 数 描述 旋转 的 原因 。 我 们 可 以 使 用 只 有 3 个 数 的 系统 ， 期 
望 看 到 结果 的 奇异 性 ， 或 者 使 用 4 个 数 处理 宛 余 。 相 应 的 选择 取决 于 应 用 和 计算 方法 。 对 于 
计算 机 应 用 ， 元 余 并 不 是 问题 ， 因 此 大 多 数 算法 都 使 用 具有 和 额外 数 的 描述 。 然 而 ， 工程师 们 
却 喜欢 用 最 少 的 组 数 进行 计算 ， 对 于 描述 旋转 而 言 没 有 唯一 性 和 超级 方法 。 


克 3.6.1 旋转 矩阵 


旋转 矩阵 表述 是 最 有 用 的 空间 旋转 的 表述 方法 ， 它 是 由 方向 余弦 确定 和 导出 的 。 具 有 共 
同 原点 的 两 个 参考 坐标 系 G 和 B 通过 单位 矢量 {G} =, J, R) 和 {B) 二 {1,7,&) WEZE 
手 定 则 所 定义 。 通 过 描述 这 两 个 参考 坐标 系 之 一 的 单位 矢量 ， 能 够 容易 发 现 两 坐标 系 之 间 的 
旋转 或 变换 矩阵 。 
f= -Di+d «pst © yk=cos(1,7)i+cosM,j)j +cosC kK (3-209) 
FHS DIAG pst + Hk=cosf itcos(f .p)j+cosh ,RE (3-210) 
K=(K + Di+(K +p) J +R ¢ )R=cos(K i) +cos(K «jg +eos(K AYR (3-211) 
因此 ， 有 旋转 矩阵 5Ra : 
Ti [ej Tek cos(I,i) cos(I.j)  cos(I,k) 
GRa=|Jei Jej Lek |=lcos(l.i) cos(J.j) cos(f ,k) (3-212) 
K.i Kej Kek cos(K 52) cos(K.j) cos(K .k) 
通过 旋转 矩阵 Ra 很 容易 找到 一 点 在 全 局 坐标 系 中 的 坐标 ， 当 这 一 点 在 局 部 坐标 系 中 的 
坐标 给 定时 。 


























































































































ul 











Sr 一 “ReBBr (3-213) 
旋转 矩阵 将 被 旋转 部 分 转换 成 矩阵 乘法 。 它 是 简单 而 方便 的 ， 特 别 是 当 其 绕 着 全 局 主轴 旋 
转 ， 或 者 绕 着 局 部 主轴 旋转 时 。 
正 交 性 是 旋转 矩阵 的 最 重要 上 且 最 有 用 的 特性 ， 它 表明 旋转 和 抢 阵 的 逆 抢 阵 等 效 于 其 转 置 矩 
KE, HISOR 一 SR 。 单 位 矩阵 工 可 用 于 描述 一 个 空 旋 转 。 
旋转 德 阵 的 主要 缺点 是 如 此 多 的 数字 通常 使 得 旋转 和 矩阵 很 难 被 解释 。 必 要 的 归 一 化 有 助 
于 建立 数值 误差 。 
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*3.6.2 fat 


由 罗 德 里 格 斯 公式 所 描述 的 角 轴 是 欧 拉 刚体 旋转 理论 的 一 个 直接 结果 。 在 这 个 方法 中 ， 
绕 着 由 单位 矢量 所 直接 表示 的 轴 旋 转 之 后 所 形成 的 具有 右手 定 则 的 旋转 幅 值 及 辐 角 8%， 可 



































用 来 描述 旋转 。 
SREp=R.i.,ys=Icosg +uu! vers +ü sing (3-214) 
通过 扩展 很 容易 将 角 轴 描述 转换 成 矩阵 形式 。 
ui vers +c UU, VErSp— U; SE UU, Verso Tu,syg 
CRg=| u, u, versg Hu, sg u; vers Hcg Uu, vers — u] so (3-215) 
UU, VErsp Tuss uusversp Tuso us vers +c 




















FHE Ea E ERE PE RA AERA 3-3 ras. R, RE ERRE 4 元 
数 ， 它 有 时 较 容 易 ; 这 时 再 将 4 元 数 转换 至 角 轴 形式 。 

角 轴 表述 有 一 些 问 题 。 A M =o 时 旋转 轴 是 非 确定 性 的 。 其 次 ， 角 轴 表 述 是 一 个 
2 对 1 映射 系统 ， 因 为 






































Ri,—sC—Ri.,y (3-216) 

并 且 它 是 多 余 的 ， 因 为 对 于 任何 整数 有 
Ri, grn SRi, (3-217) 
然而 ， 通 过 重新 限定 $ 至 一 些 合适 范围 ， 如 [0，z] 或 者 | A = |e 定 程度 上 ， 





这 些 问题 会 得 到 改善 。 最 后 角 轴 表述 对 于 求解 两 个 旋转 的 构成 和 确定 等 效 的 旋转 角 轴 并 不 是 
很 充分 。 


*3.6.3 RHR 


欧 拉 角 仅 仅 只 利用 3 个 数 ， 也 可 用 来 描述 刚体 旋转 矩阵 。 
cocy—cOsgsp copsy— cIcysp sse 
cWSD 十 cOcpsW 一 SPSV 十 cgOcpcW —cepcl (3-218) 
sO sp SOcp cO 
欧 拉 角 和 旋转 甜 阵 通常 并 不 是 1 对 1 的， 它们 也 不 是 一 个 有 关 旋 转 的 简便 表述 ， 或 者 说 
也 不 是 构建 合成 旋转 的 简便 表述 。 当 0 一 0 时， A p Fld 没有 区 别 。 
和 矩阵 相 乘 可 直接 获得 等 效 的 旋转 矩阵， 与 此 同时 从 旋转 矩阵 至 欧 拉 集 的 逆 变 换 并 不 简单 
明确 。 当 09>0 时 ， 它 也 是 不 可 用 的 。 通 过 式 (3-218) 能 够 求 得 如 下 欧 拉 角 。 





CR, = [R.yR20Rz.¢ |= 















































p=—arctan{ “| (3-219) 
r93 
0=arccos(r,, ) (3-220) 
— r31 
parctan| ) (3-221) 
1259 








有 可 能 会 使 用 一 个 能 够 统一 处 理 所 有 情况 的 方法 。 这 个 思想 将 通过 o 和 y 的 和 差 起 作用 。 
o=otry (3-222) 
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v=o 4 (3-223) 
这 时 ， 有 
ov 
p= (3-224) 
2 
a (3-225) 
2 
因此 ， 有 
ri 十 ro 二 cosa(1 十 cosO) (3-226) 
rii ~ro —cosv(1—cosé) (3-227) 
To “r; =sino (1+ cos) (3-228) 
ro, Frig =sinv(1—cosd) (3-229) 
这 样 一 来 ， 则 有 
ra Tie 
o =arctan (3-230) 
11 1 122 
Tat UT ie 





(3-231) 


v =arctan = 
Pi "22 


这 个 方法 可 解决 在 0=0 时 的 问题 。 当 0=0 时 可 求 得 so， 但 是 vv 不 能 确定 。 当 06 二 x 时 
我 们 可 以 求 得 v, 但 是 so 不 能 确定 。 不 能 确定 的 值 是 由 于 arctan 让 所 导致 的 。 除了 这 些 特 殊 
情况 外 ， 可 单独 确定 0 和 zw。 通过 反正 切 运算 也 能 求 得 中 间 旋 转角 0。 

ri1s SINP TT 2s oF 
7 33 

欧 拉 角 的 主要 优点 是 它们 仅 使 用 3 个 数 。 欧 拉 角 是 可 积分 的 ， 并 且 为 具有 无 兄 余 性 的 空 
间 旋 转 提供 一 个 很 好 的 可 视 化 。 欧 拉 角 也 用 于 自 旋 刚体 的 动力 学 分 析 之 中 。 

欧 拉 角 的 其 他 合成 与 翻 深 角 、 俯 仰角 和 偏 航 角 有 着 相同 种 类 的 问题 和 类 似 的 优点 。 
k3.6.4 四 元 数 


依据 四 元 数 的 加 法 和 乘法 ， 四 元 数 利 用 4 个 数 表述 旋转 。 旋 转 四 元 数 是 一 个 单位 四 元 
数 ， 它 由 欧 拉 参数 或 者 旋转 轴 和 旋转 角 来 描述 。 




















g=aretan{ (3-232) 






































e($.u) =e, t+E=e,+e,ite,j +e,k =cos 9 Fsin Éa (3-233) 
我 们 也 可 以 定义 4X4 和 矩阵 来 描述 四 元 数 。 
do Q Gg ds 
A (3-234) 
d2 43 qo qi 
d3 Ge qı do 
该 矩阵 提供 了 一 个 重要 的 正 交 特性 。 
q =q" (3-235) 


AE M4: PU sca (3-24) 也 可 用 式 (3-236) 表述 。 
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(3-236) 


(3-237) 


(3-238) 


(3-239) 


这 里 ， 
0 Tg q 
q=| 9, 0 =q 
=q q 0 
利用 和 抢 阵 四 元 数 7 ， 我 们 可 以 通过 年 阵 乘法 描述 四 元 数 乘法 。 
qo Tq Ta Ta \ Po 
二 gq1 qo Tdh Aa ||P; 
gp = P = 
d2 3 qo ~~ 41 || P2 
d3 T4? qı qo J \p; 
四 元 数 的 矩阵 描述 与 四 元 数 运算 和 算 阵 运算 有 关 ， 因 为 如 果 p、g 和 w 是 3 个 四 元 数 ， 
并 且 有 
qp 5v 
那么 ， 有 
es 





因此 ， 两 个 坐标 系 之 间 的 四 元 数 变换 表述 
Sr=e(¢ ou) Pre” (8 su) 
也 能 够 用 矩阵 相 乘 的 形式 来 定义 ， 


Cr =e (gr re ASE ($a) Pre (piu)? 








UE AA : 
我 们 可 以 利用 四 元 数 的 和 矩阵 定义 有 
CE 
ee A ei ep — e3 ez 
e ($ ,2 ) 一 
€z E3 &o ey 
Ga 3 e] Ci 
0 Bi Br By 
Biej 0 — Br, 
BP 3 
ar Bz 0 i, 
By 一 Bri 0 
€ ej €z es 
es 二 iy eg es BB 
e (¢ su) = 
eg eg eo Ei 
— e3 ez “ey eg 





因此 ， 有 
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(3-240) 


(3-241) 


(3-242) 


(3-243) 


(3-244) 


(3-245) 














0 Gy Gr ors 
sep ene a 0 7 Sr, 
e(¢.u)®re(d.u)t=| . _ (3-246) 
G 7 Sr, 0 pe 
Gy Cia or, 0 
这 里 
Gr = Pr Ce? He? e? e?) HBr, (2eje, eyes or (eges F2eje,) (3-247) 





Gr, = Er (2e e; + 2e,e,) +r, (2e? e? | e? e?) HBr, (2e,e, F2ege;) (3-248) 
Gr, By (2eje; —2ege,) HBr; (ege; H2eze,) HBr, Ce? —e?—2e? te?) (3-249) 
它们 与 式 (3-178) 是 一 致 的 。 


妇 3. 6.5 欧 拉 参数 


欧 拉 参数 是 旋转 四 元 数 的 元 素 。 因 此 ， 在 旋转 四 元 数 与 欧 拉 参数 之 间 存 在 着 一 个 直接 变 
换 ， 该 变换 依次 与 角 轴 参数 相关 。 通 过 欧 拉 参 数 或 者 旋转 四 元 素 el, zw)， 可 以 获得 相应 旋 
转轴 和 旋转 角 C> ù). 











le | 


多 一 2arctan (3-250) 
0 
i=] (3-251) 





单位 四 元 数 对 于 空间 旋转 描述 提供 了 一 个 很 好 的 基础 ， 尽 管 由 于 误差 累积 问题 而 需要 标 
准 化 。 总 之 ， 在 一 些 应 用 中 ， 四 元 数 具 有 非常 高 的 计算 效率 。 

有 趣 的 是 ， 欧 拉 是 第 一 个 导出 罗 德 里 格 斯 公式 的 人 ， 而 罗 德 里 格 斯 是 第 一 个 发 明 欧 拉 参 
数 的 人 。 此 外 ， 哈 密 尔 顿 (Hamilton〉 引 入 了 四 元 数 ， 然 而 高 斯 (Gauss) 也 发 明了 四 元 
数 ， 但 却 没 有 报道 出 来 。 

K Gil 3-30 ”旋转 矩阵 的 泰勒 级 数 扩展 形式 。 

假设 旋转 矩阵 R= 二 RG()， 是 一 个 在 坐标 系 BAG 之 间 的 有 关 时 间 的 变换 。 刚 体 坐 标 系 B 
在 :一 0 时 与 坐标 系 G 重合 。 因 此 , ROSI, FARMER 中 的 元 素 扩展 成 一 个 泰勒 
级 数 扩展 形式 。 


























1 1 
R(t) =I1+Rit+=—R2t? 3 Rat? Feee (3-252) 


式 (3-252) 中 , R;G=1, 2, 3, =) E— NE RUE. WEP FEM Rt) 对 于 所 有 时 间 
t 都 是 正 交 的 ， 因 此 ， 有 


























RRT=I (3-253) 
1 1 
i + Rit 5 Ret? | -)(1 -RI 5 Ree? | =! (3-254) 
在 式 (3-254) 的 左边 ,ti(i 二 1]，2，3，…) 的 系数 必须 化 为 零 ， 则 有 
R,+R{ =0 (3-255) 
R.+2R,Ri +R; =0 (3-256) 
R;+3R2R{+3RiR; +R} 一 0 (3-257) 


或 者 概括 地 写 为 : 
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5 (rr? =0 
2 


i=0 
这 里 
Ro =Ri=I 


(3-258) 


(3-259) 


st (3-255) 表明 R1 是 一 个 和 斜 对 称 和 矩阵 ， 因 此 ，RIiRTI 王 一 R: =C, 是 对 称 的 。 现 在 ， 
st (3-256) 可 写 为 








及 > 十 Ri =—2RıR] =—[R:R] +RıR])"]=2C; (3-260) 
有 
有 > 一 Ci 十 [C1 一 Ri ] (3-261) 
Ri =Cr-+C,— Ri |= Ci + (CE: R] ]" (3-262) 
式 (3-262) 表明 ，[Ci 一 Ri ] 是 斜 对 称 的 ， 因 为 
[C, —R} ]+[C:—R}]'=0 (3-263) 


因此 ， 和 矩阵 乘积 








[Ci—RIJ[Ci1—R1]'=—[Ci1—Ri] (3-264) 
是 对 称 的 。 下 一 步 说 明 式 (3-265): 
R; +R] 一 一 3LRIRL #R:R] ]-=—3[R:R] +(RıR7)"] 














=3(Ri (R? —R})+(R?—R})Ri ]=2Cz (3-265) 
由 式 (3-265) 可 得 
R,=C2+[C2—R3 ] (3-266) 
R} =C2+(C2—R3 ]=C2+[C2—R3 ]" (3-267) 
式 (3-267) RIH, [CRI] 是 斜 对 称 的 ， 因 此 
[C2—R3z J+ [C2—R}i ]’=0 (3-268) 
因此 ， 和 矩阵 乘积 
[C2—R3 JLC2—R3 J'=—LC2—R3 了 (3-269) 


可 以 看 出 ， 式 (3-269) 也 是 对 称 的 。 
继续 这 个 过 程 表明 旋转 矩阵 RG(1) 的 扩展 形式 可 以 写 为 











1 1 
R@)=I+ Cit zC! CCIR?) jz? a1LC? FC2 —RE) J (3-270) 
这 里 ，C; ÆR, [CSR a] 是 斜 对 称 和 矩阵 ， 





Ci= [R HRI] (3-271) 
EE, DE DEFE IE PERI D RIRE S RAN F JÉ : 


1 1 
R?’Q@)=I1+Cit4 Lei +(C1—R}) ]t?4 zi Ce HF(Cs—RI)J (3-272) 














克 3.7 旋转 的 合成 与 分 解 





具有 国定 点 的 刚体 绕 着 ú, 轴 旋 转 $1 RERE &， 轴 旋转 % ， 该 旋转 可 被 合成 为 一 个 
单独 绕 着 n, 轴 的 旋转 角 $，。 换 句 话说 ， 当 一 个 刚体 从 初始 位 置 旋转 到 一 个 中 间 位 置 
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Pr 二 BRp,B1r ， 然 后 旋转 到 一 个 最 终 位 置 wr = Rp Pr 时 ， 中 间 位 置 可 以 被 跳 过 ， 直 接 
eee RAMA r =" Rp Br 。 

证 明 : 

为 了 说 明 具 有 固定 点 刚体 的 两 个 连续 旋转 等 效 于 一 个 单一 旋转 ， 我 们 可 从 罗 德 里 格 斯 旋 
转 公 式 (3-137) 开始 ， 将 该 式 重 写 为 












































(r'—r)=w X(r' +r) (3-273) 
这 里 ， 
w =tan Sa (3-274) 
它 是 罗 德 里 格 斯 矢量 。 先 旋转 wl ， 后 旋转 w* ， 则 分 别 有 
《Cry 一 ri J=w, X (r, +r) (3-275) 
(ry =r) =w, X (r, Fr,) (3-276) 
第 1 个 方程 ， 即 式 (3-275) 的 右边 垂直 于 w; 。 第 2 个 方程 ， 即 式 (3-276) 的 右边 垂直 于 








w, Alt, Aw 的 式 (3-75) MEA w, AII (3-276) 的 点 积 表明 : 


Wi * row, F] (3-277) 
W, ° r; =W, * rT, (3-278) 
它们 的 又 积 表明 : 
wo X (r, —r;) =w, X66rs r=w L[w, ° (r, tri) Cw * w,) r, +r) 
—w, |w; © r; +r) ] +w, * w) r, +r) (3-279) 


利用 式 (3-277) 和 式 (3-278), JAMS (3-279) ， 可 得 
w, X ro =r) =w X Or, =r) = (wy XW) X (ri +r) + Cw, 。W)Cri 一 r) (3-280) 
xt (3-280) 可 写 为 : 
(w +W,) Xr, =w,Xr; tw Xr; (w, Xw) tr) + Ow, ° w,) r; =r) (3-281) 
式 (3-275) 与 式 (3-276) 相 加 ， 可 获得 (Cw, 十 w,) Xr,， 则 有 








(w WwW.) Xr, =r; —r; Ww, Xr, tw, XP (3-282) 
因此 ， 我 们 可 获得 所 要 求 的 罗 德 里 格 斯 旋转 公式 ， 将 rr, 旋转 变换 至 -3 : 
r,—r,=w, X(r,+r,) (3-283) 
这 里 ， 
wi tw, +w, Xw] 
w= (3-284) 
1—-w,°wW, 





具有 国定 点 的 刚体 绕 着 co 轴 旋 转 的 任何 角 g, 可 以 被 分 解 为 3 个 连续 的 旋转 ， 通 过 特 
定 的 角 a、B 和 7 分 别 绕 着 任意 轴 4、5 和 < 旋转 。 

假设 SRs.。、5Ri.s 和 SR,y 是 绕 着 非 共 轴 非 共 面 的 单位 矢量 4、0b 和 < 进行 旋转 的 按照 次 
序 的 任意 旋转 矩阵 ， 其 中 a。、B 和 7 是 非 消除 值 。 这 时 ,任意 的 其 他 旋转 变换 矩阵 5R;,y 可 
HW Rie CRE e ACR: AN, WA a, BMY 是 正确 的 选择 数字 的 话 。 





GRa,s—=°Re,y Re,s Ra, (3-285) 
证 明 : 
利用 基于 四 元 数 的 旋转 变换 定义 ， 我 们 可 以 写 出 : 
Sr =C R}, Pr =el. ú)Pre*(ġ.úù) (3-286) 
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假设 表示 在 旋转 之 前 的 位 置 矢量 r ,，r,、r 3 和 7 ,分 别 表示 在 旋转 SR,,。 Rog 








和 SR 之 后 的 位 置 矢量 。 因 此 ， 有 


= * 
r,—ar,a 























r,—br,b* 

r,=cr,c* 

r,=er,e* 
这 里 ， 

So as i ME a a 
a(a,a)=a,t+a=cos z Tsin ya 
b(B.b)=b, b=cos sin ie 

2 2 

oe oe is ae PS 
c(Y +6) =cy +e=cos z Tsin y 
hae _ $, $a 
e(p.u)=e, te cos sg tsin su 


IÈ (3-287) ~È (3-290) 就 是 四 元 数 ， 分 别 相当 于 (a, 4)、(B, E OY, &) 和 


我 们 定义 下 列 标量 ， 以 简化 方程 。 











668 本 二 Cl cos E =C: cos 了 一 (C3 cos $=C 
sin —=S, sin =s, sin + =Ss sin% =s 
025c3 一 0352? 203c1 一 0153 | bic bo > 

oe. 9° ae 9° fis, 
C243 CC342 C143 ~ C34] C241 Cas 
5 
Q203 一 4302? 4301 一 4103 a;b 一 4201 
Ba <a . [o> 
b +c 5n S:S; 


c sa =n,S351 
a * b =n, SiS? 
(a Xb) +e =n, Si S283 
直接 替代 ， 则 有 
ri=erie =cbhar,a* b*c* 
因此 ， 有 
e =cba =c (bab +a tb ata, b+tbXa) 








=c boao agb *e—b c *a—c a +b +(aXb)re ta boe tb coa tcaagb + 
ay (b Xe) +b, Ce Xa) +c, (bXa)—(a *b)ec —(b +e) sate a)b 


因此 ， 有 
eo—=coboao aoniS2Ss—bn,Ss3S1—consS1S2 TnsS1S2Ss 





e=a boc Fbocoa Fcoagbhb ta bXe) tb, (eXa)+c, (bXa) 
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(3-287) 
(3-288) 
(3-289) 
(3-290) 


(3-291) 
(3-292) 
(3-293) 
(3-294) 
Ge De 
(3-295) 


(3-296) 


(3-297) 


(3-298) 


(3-299) 


(3-300) 
(3-301) 
(3-302) 
(3-303) 


(3-304) 


(3-305) 


(3-306) 





—n,S2S3a +n,S3Sib 一 239192C 


式 (3-307) 产生 了 4 个 四 元 数 。 


C1C2C3—n,C1C2C3—n, 818283 Hn S1S2S3—n,S1S2S3 =C 
a S1C2C3 7b, C1 S283 Hc}C1C2S3 7 f,C18283 Hg S1C283 FhiSiS2C3— 


nja; Sı S283 Hn b; S1 S283 一 713C1 


Sı S283 5u; S 


a,S1C2C3 +b,C1S2S3 +c;C1C2S3 +f ,CiS283+g,51C2S3 +h2S182C3— 


nja Sı S283 +n,b, S 


S2S3—n3C, 


S1S283=u,5 








a S1C2C3 1+6,C1S2S83 
nja; Sı S283 +n, b, S 








re,CiC2S3trf,Ci1S2834 
S2S3—136,518253=u,8 





因为 e? 十 e? 十 e2 十 e2 一 1， 因 此 只 有 第 1 个 方程 和 沿 着 


Gi=4 lS; C= 0 3 








K Bil 3-31 非 正 交 坐标 系 中 的 矢量 分 解 。 








的 其 他 两 个 方程 能 够 被 用 来 决定 C1、C，。、C3、S1、S。 和 S;。 





tg ,S1C2S3+h381S2C3— 


(3-308) 


(3-309) 


(3-310) 


(3-311) 


(3-312) 


假设 a、b 和 c 是 任意 非 共 面 的 不 能 化 为 零 的 矢量 ， 如 果 u, v 和 w 是 正确 的 选择 数字 ， 








则 任何 其 他 矢量 r 能 够 通过 a 、b 和 c 矢 量 被 表达 














r=ua+vb+we 
r=(reDI+G@-eJ)J +a :KK 








为 了 展示 这 个 ， 我 们 通过 (bXC) 从 寻求 式 03-13) 的 点 积 开 始 。 


re (bXc)=ua* (bXec)+vb-: (bXe)+we: (bXe) 














(bXc) 垂直 于 矢量 和 矢量 ce ， 因 此 有 














r°» (bXce)=ua* (bXe) 


因此 ， 


_ [rbe] 
“ Uabc] 





b 
HE 


[abc]=a + bXc=a*+ (bXe)= a, by Cy 


类 似 地 ， U All w 将 是 : 
_ [rea | 
° [abe | 
_ [rab] 
~ [abe | 





W 


因此 ， 
_ [rbe] [rca] 





a, bi c 


a, 03 C3 


_ [rab] 





[abe |’ ' [abe | 
st (3-321) 也 可 写成 


Tabe]? 


(3-313) 


(3-314) 


(3-315) 


(3-316) 


(3-317) 


(3-318) 


(3-319) 


(3-320) 


(3-321) 
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_{ bxce i cXa _(.aXb 7 
r (r re ja | (r ca jo | (r eee (3-322) 
sh (3-22) 两 边 同 乘 以 [abcej， 则 得 一 个 对 称 方程 : 
Labe |r —Lber je 十 Lera |b —[rab |e =0 (3-323) 








WR (a, b, c) 坐标 系 是 一 个 笛 卡 儿 坐 标 系 (i，，J ，K 太 ) 是 一 个 它 的 单位 矢量 互相 
正 交 的 系统 ， 则 有 : 


[iJK]=1 (3-324) 
Ixfj=K (3-325) 
jxK=I (3-326) 
KxIi=jJ (3-327) 
式 (3-322) 变 为 
r 一 Cr .1 站 DT 二 Cr -fjr KK (3-328) 


3.8 本 全 小 结 


本 章 的 目标 如 下 ; 

D SARA BBS =u, u, uT 旋转 #$ 角 时 ， 确 定 在 具有 共同 原点 的 两 直 
JAIRA B 和 G 之 间 的 旋转 变换 矩阵 。 

D 对 于 一 个 给 定 的 变换 矩阵 ， 求 取 其 旋转 角 ”和 旋转 轴 c2。 

考虑 具有 共同 原点 的 两 直角 坐标 系 B 和 G。 我 们 可 以 使 坐标 系 BAA oR AOA a 
旋转 $ 角 ， 将 坐标 系 也 转换 成 坐标 系 G。 已 知 旋转 轴 和 旋转 角 ， 可 通过 罗 德 里 格 斯 旋转 公式 






























































计算 变换 矩阵 。 
SRB 王 Ra 一 Icosg 十 LTIvers 风 十 下 Sing (3-329) 
另 一 方面 ， 我 们 能 够 从 给 定 的 旋转 矩阵 "Res 求 得 旋转 角 和 旋转 轴 。 
1 5 
cosg => (tr Rp) 1) (3-330) 
@=—>—(GR,—€R?) (3-331) 
2sing B 
旋转 角 和 旋转 轴 以 及 罗 德 里 格 斯 旋转 公式 ， 也 可 通过 欧 拉 参数 egn ejn egn e, 进行 
定义 。 
e,=cos É (3-332) 
e=e, Íte, f +e, R =ùsin É (3-333) 
ERg=R3, = (e? —e’ 1+ 2ee'+ 2e € (3-334) 
或 者 通过 单位 四 元 数 来 定义 : 
e(¢,u)=e, te =e, te,ite,j +e,k =cos 5 F sin Éa (3-335) 





式 (8-335) 可 为 定向 分 析 提 供 一 些 优 势 。 
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习题 


3-1 标记 与 符号 。 
描述 下 列 这 些 符号 的 含义 。 


1) 2) vers¢ 3) a 4) Ray 5) e, 6) e 
7) BRG! 8) dy 9) q tq 10) ij 11)q* 12) e(¢, ù) 
13) J 14) g 15) q 16) |g| 17) R3X3 


3-2 旋转 不 变 轴 。 

确定 是 否 旋 转 要 被 固定 在 B(COzyz) 坐标 系 或 者 GCOXYZ) 坐标 系 中 。 
3-3 z 轴 角 的 旋转 矩阵。 

扩展 轴 角 旋转 矩阵 
GRB 一 BR51LI 一 BRE 一 Ri 一 (4 -pA y, OAy04p)T 一 4T Al Al AT Al, 
验证 轴 角 旋转 矩阵 。 


























uj vers¢ +c¢ U| U, VErSp — U, SP uiusversg Tu,sg 
GRg=R;,¿ =| u u, versg Fu, so u‘ vers +c u, u, vers —uisg 
U,Uz,VErSP—UySP Usu, vers TU, Sh uz verse tc¢ 
3-4 轴 角 分 解 。 


刚体 坐标 系 BRE X 轴 旋 转 30"， 然 后 绕 着 Z 轴 旋 转 45°. 

1) 试 确定 旋转 变换 矩阵 5 Rs。 

2) 试 确定 能 提供 相同 旋转 变换 矩阵 5 Rs 的 旋转 角 和 旋转 轴 。 

3) 试 确定 6Rp 的 欧 拉 角 。 

4) 试 确定 Ra 的 欧 拉 参 数 。 

K3-5 x HS ERE EE 

将 z HAE RB re Pe A E OR LP He Fe RE. 

K3-6 y 轴 角 旋转 矩阵 。 

将 y 轴 变 换 到 旋转 轴 之 上 ， 试 求 轴 角 旋转 矩阵 。 

3-7 hfa DEFE AIRRA ff 

1) 试 求 与 绕 着 矢量 & = (1 1 1)7 旋转 45" 相 当 的 欧 拉 角 。 

2) 对 绕 着 x 轴 旋 转 45" 然 后 绕 着 y 轴 旋 转 45" 再 绕 着 x 轴 旋 转 45" 的 综合 旋转 而 言 ， 试 
确定 其 旋转 轴 和 旋转 角 。 

3) 试 求 与 2) 中 旋转 相当 的 欧 拉 角 。 

4) 试 求 与 1) 中 具有 相同 的 最 终 方位 且 先 后 分 别 绕 着 x 轴 、y 轴 、z 轴 旋 轴 旋 转 的 所 
要 求 的 角 。 

3-8 两 局 部 坐标 系 之 间 的 欧 拉 角 。 

两 坐标 系 Bi 和 G 之 间 的 欧 拉 角 是 20"、35" 和 一 40"。 坐 标 系 Bo MC 之 间 的 欧 拉 角 是 
60"、 一 30" 和 一 10"。 试 求 将 坐标 系 Bo 变换 到 Bi 的 旋转 角 和 旋转 轴 。 

3-9 ”立方 体 的 全 局 旋转 。 
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图 3-5 所 示 为 固定 在 A 点 且 棱 长 /一 1 
的 立方 体 的 初始 位 置 。 

D 将 立方 体 绕 着 直线 AC 旋转 45"， 试 
确定 顶点 的 全 局 坐标 。 

2) 将 立方 体 绕 着 直线 AH 旋转 45°, ik 
确定 顶点 的 全 局 坐标 。 

D 将 立方 体 绕 着 直线 AG 旋转 45°, ik 
确定 顶点 的 全 局 坐标 。 

3-10 系列 的 旋转 轴 角 。 

在 图 3-5 中 ， 立 方 体 固 定 在 A 点 ， 并 且 
其 棱 边 长 为 /一 1。 

D 当 我 们 将 立方 体 绕 着 x 轴 旋 转 45°, 
接着 再 绕 着 y 轴 旋 转 45" 时 ， 试 求 旋转 角 和 图 3-5 在 其 初始 位 置 处 且 楼 长 1=1 的 立方 体 
旋转 轴 。 

2) 当 我 们 将 立方 体 绕 着 x 轴 旋 转 45"， 接 着 再 绕 着 直线 AH 旋转 45" 时 ， 试 求 转 角 和 旋 
转轴 。 

3-11 立方 体 旋 转 的 全 局 分 解 。 

在 图 3-5 中 ,立方体 固定 在 A 点 ， 并 且 其 棱 边 长 为 /一 1。 

1) 将 立方 体 绕 着 直线 AG 旋转 45 分 解 成 分 别 绕 着 X 轴 、Y 轴 和 2 轴 的 一 次 旋转 。 

2) 将 立方 体 绕 着 直线 AG 旋转 45" 分 解 成 分 别 绕 着 Y 轴 、2Z 轴 和 X 轴 的 一 次 旋转 。 

D 将 立方 体 绕 着 直线 AG 旋转 45" 分 解 成 分 别 绕 着 Z 轴 、Y 轴 和 X 轴 的 一 次 旋转 。 

KA) 将 立方 体 绕 着 直线 AG 旋转 45" 分 解 成 分 别 绕 着 AC HAA 轴 的 一 次 旋转 。 

3-12 立方体 旋转 过 程 中 一 2Z 的 体积 。 

在 图 3-5 中 ， 立 方 体 固定 在 A 点 ， 并 且 其 棱 边 长 为 7 一 1。 

1) 将 A 移动 到 X 轴 ， 对 于 轴 AG， 它 的 旋转 角 应 该 是 多 少 ? 

K2) 在 旋转 6 角 期 间 ， 试 计算 立方 体 具 有 一 2 坐标 的 立方 体 的 体积 。 

3-13 绕 着 一 个 固定 轴 和 一 个 固定 刚体 轴 立 方 体 的 旋转 。 

在 图 3-5 中 ,立方 体 固定 在 A 点 ， 并 且 其 棱 边 长 为 一 1。 

1) 将 立方 体 绕 着 直线 AC 旋转 45" 然 后 再 绕 着 AH 旋转 45"。 试 确定 旋转 之 后 立方 体 顶 
点 的 全 局 坐标 。 

2) 将 立方 体 绕 着 直线 AC 旋转 45 "然后 再 绕 着 AG 旋转 45"。 试 确定 旋转 之 后 立方 体 顶 
点 的 全 局 坐标 。 

K3) 将 立方 体 绕 着 直线 AC 旋转 45" 然 后 绕 着 AG 旋转 45 接着 再 绕 着 直线 AH 旋转 
45"。 试 确定 旋转 之 后 立方 体 项 点 的 全 局 坐标 。 

WS-14 基于 欧 拉 角 的 旋转 角 和 旋转 轴 。 

将 欧 拉 角 旋转 矩阵 与 轴 角 旋转 矩阵 相 比 较 ， 试 求 基 于 欧 拉 角 的 旋转 角 和 旋转 轴 。 

克 3-15 ”基于 旋转 角 和 旋转 轴 的 欧 拉 角 。 

将 欧 拉 角 旋转 矩阵 与 轴 角 旋转 矩阵 相 比 较 ， 试 求 基 于 旋转 角 和 旋转 轴 的 欧 拉 角 。 

六 3-16 重复 全 局 -局 部 旋转 。 
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将 矢量 arp 一 (6 2 


— 3)" RA Z 轴 旋 转 60 然后 再 绕 着 x 轴 旋 转 30  。 然 后 重复 该 旋 


转 顺序 。 旋 转 多 少 次 之 后 点 P 返回 到 它 的 初始 全 局 位 置 ? 


克 3-17 重复 全 局 -局 部 旋转 。 








绕 着 X Hee a=180°/m, HABA > HEF B=180°/k, Wm, REN, WE: 旋 





转 多 少 次 之 后 刚体 点 可 被 带 回 到 它 的 最 初 全 局 位 置 ? 








克 3-18 微小 旋转 角 。 


证 明 : 对 于 绕 着 局 部 坐标 轴 旋 转 wg、9 和 y 的 微小 角 ， 当 它们 绕 着 同一 旋转 轴 时 第 


转 和 第 3 旋转 是 无 法 区 分 的 。 























1 旋 




















%3-19 a 的 内 部 自 同 构 特征 。 
如 果 R 是 一 旋转 矩阵 ， 并 且 a 是 一 个 矢量 ,证明 : 
RaRT=Ra 
43-20 主 旋 转 矩阵 的 角度 导数 。 
TEAR : 
dRz,a 一 dRy.g 一 dRx,y _ ~ 
da =KRz,a, dp =J Ry. dy =I Rx. 
Ww3-21 欧 拉 角 ， 欧 拉 人 参数 。 
比较 欧 拉 角 旋 转 和 矩阵 和 欧 拉 参 数 变换 和 矩阵， 验证 在 欧 拉 角 和 欧 拉 参 数 之 间 的 下 列 关 系 : 
e) =cos 2 cos te 
e, =sin en e 
EO 2 
e, =sin sin P 
_ 0. gyte 
e3 =cos ysin “> 
2(e,es Fee) 
= arccos sind 


0=arccos[2(e? +e? y-1] 


一 2(eyey ~ege) 








sin@ 


T 
=y 


由 二 arccos 
友 3-22 四 元 数 定义 。 
求 与 下 列 参数 相关 联 的 单位 四 元 数 e(%， 习 )， 其 中 ， 
1/43 
ù= | 1/43 
1/43 





并 求 e(8，, ù) ie” (G, a) 的 结果 。 
K3-23 ”四 元 数 的 乘积 。 
如 果 





p=3+i—2j +2k 
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dg 一 2 一 ;十 27 +4k 
r 一 一 1 十 ;十 ) 一 3 


WOR pq. ah. Pa. abs PP. ady PY 以 及 pra’. 
3-24 四 元 数 的 逆 。 
如 果 





p=3ti-2j+2k, q=2—i+2j+4k 
试 求 q ys p's pg ty a PR. prs ee DR lg 1!。 
WS-25 四 元 数 和 角 轴 旋转 。 
求 与 下 列表 达 式 相关 联 的 单位 四 元 数 : 
pH3ti-2j+2k, q=2—i+2 
同时 求 出 每 个 单位 四 元 数 的 旋转 角 和 旋转 轴 。 
W3-26 单位 四 元 数 和 旋转 。 


























使 用 单位 四 元 数 p : 
1 十 i 一 j +k 
a a 
试 求 矢量 ar 的 全 局 位 置 。 
2 
Br =| —2 
6 


3-27 四 元 数 和 矩阵 。 
使 用 与 下 式 相 关 的 四 元 数 和 矩阵 





p=3+i-—2j 十 2k 
gq 二 2 一 i 十 2j +4k 
r=—1+itj—3k 





REFI Fp PI IP PE E PETE 
K3-28 欧 拉 角 和 四 元 数 。 

根据 欧 拉 角 求 四 元 数 分 量 ,， 根 据 四 元 数 分 量 求 欧 拉 和 角 。 
3-29 角速度 矢量 。 








利用 定义 GRs 一 [x ] 和 6 Re=Lr, ]， 求 角速度 m ， 其 中 O=FRORE, 





友 3-30 bac-cab 规则 。 

使 用 利 维 - 奇 维 塔 (Levi- Civita) 密度 es 水 证 明 bac-cab 规则 。 
ax (bXc)=b (a+c)—c (as bd) 

克 3-31 bac-cab 规则 应 用 。 

使 用 bac- cab 规则 证 明 











a=n(aen)+nX(aXxn) 

其 中 是 任何 单位 矢量 。 该 方程 的 几何 意义 是 什么 ? 

K3-32 两 次 旋转 并 不 充分 。 

证 明 : 通常 ,不 可 能 通过 绕 着 不 同 的 全 局 主轴 进行 三 次 旋转 将 来 自 

















初始 位 置 P(X;， 








Yi; Zi) 的 一 点 P(X, Ys Z) EE A oh) Bll te ZA i EP CX, Yrs Lads 


er 
+ 
tn 


3-33) 三 次 旋转 是 充分 的 。 

证 明 : 通常 情况 下 ， 通 过 围绕 不 同 全 局 轴 进 行 三 次 旋转 有 可 能 将 来 自 初始 位 置 P(X;， 
Zi) 的 一 点 PCX，Y，2Z) 移动 到 最 终 位 置 PCXr，Yr，2Zr) 。 

3-34 封闭 特性 。 

证 明 转 换 和 矩阵 的 封闭 特性 。 

3-35 两 个 正 交 和 矩阵 之 和 。 

证 明 : 两 个 正 交 抢 阵 之 和 通常 情况 下 不 是 一 个 正 交 抢 阵 ， 但 是 它们 的 积 是 正 交 抢 阵 。 
3-36 SRNR, 

证 明 : 如 果 a =la, a, az)" Mb =b b: 03)? 是 两 个 任意 矢量 ， 并 且 


























a 相当 的 斜 对 称 和 矩阵 ， 那 么 





ab =a Xb 

K 3-37 和 斜 对 称 和 矩阵 。 

fi Fla=(a, a, az)" b=; b, 6,)7 TER: 
1) ab 一 一 5a。 

2) (a +b) =a +b, 





3) (ab) =ba’—ab’. 
3-38 eR FE 
WEH: 如 果 A、B 和 C 是 三 次 旋转 和 矩阵， 那么 
1) (AB)C=A(BC)=ABC, 

2) (4 十 B)T 一 4T 十 BT。 

3) (AB)T=BTAT, 

4) {AP SAT), 

3-39 和 斜 对 称 和 矩阵 乘法 。 

证 明 : 


1) alIa1I 一 一 4I0 一 0， 











2) ab =ba'—a'bdl. 

3-40 和 斜 对称 和 矩阵 导数 。 

WEH: =à 

*3-41 A=(a a) 的 时 间 导 数 。 

假设 a 是 一 个 时 间 矢 量 , 并 且 A 二 (a a) 是 一 个 3X4 HERE, CAAT 的 时 间 导 数 是 








什么 ? 


K3-42 综合 的 角 轴 旋转 。 
Savi u, 轴 旋 转 6, fl. HAM a, Whee 6, A. MAFRA a MIE p A. WHE gn 
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ù, p, Mü, KAE 6 MEHEA ú. 
K3-43 罗 德 里 格 斯 矢量 。 
利用 罗 德 里 格 斯 旋转 公式 证 明 : 

















7 一 r 一 tan 了 


2 
友 3-44 等 效 罗 德里 格 斯 旋转 矩阵 。 
证 明 : 罗 德 里 格 斯 旋转 矩阵 
SRB 王 Icosg 十 UTvers$ 十 Using 也 可 以 写 为 
CRp =I+ (sing )u + (versd )u? 
友 3-45 REE HEE A FB TN sk 
已 知 罗 德里 格 斯 公式 的 另 一 种 定义 : 
SRg=I+ (Csin 风 ) 下 十 (Vers 由 ) 下 2 


u2”-!=(—]1)""'ag, u?” =(—1)"— li 2 

















检验 如 下 方程 : 
GRESRs= GRESRE 
友 3-46 罗 德 里 格 斯 公式 应 用 。 
利用 罗 德 里 格 斯 公式 的 男 一 种 定义 : 
CRg=I+ (sing)U (vers )u? 
REG ù 旋转 45" 之 后 在 Br = 二 (1 3 4)T 处 刚体 点 的 全 局 位 置 。 
上 一 (1/V3 1/43 1/V3)7 




















3-47 旋转 轴 和 旋转 角 。 








对 于 下 列 的 变换 矩阵 ， 求 旋转 轴 和 旋转 角 。 
3 od 
4 4 4 
R- 56 2 项 
4 4 4 
1 二 3 
4 4 4 

















K3-48 旋转 轴 的 乘积 

HEHH : 

1) #t+1=(—1)*@, 

2) w =(—1) d> úúu") 

3-49 ”斯 坦 利 方法 

基于 斯 坦 利 方法 ， 求 下 列 旋转 矩阵 的 欧 拉 参数 。 
0.5449 —0.5549 0.6285 

GRg=| 0.3111 0.8299 0.4629 

—0.7785 —0.0567 0.6249 

















3-50 斯 坦 利 方法 。 
在 图 3-5 中 ， 立 方 体 在 A 处 有 一 固定 点 ， 并 且 其 棱 边 长 为 ! 王 1。 
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1) 确定 围绕 AG 旋转 45° 的 变换 矩阵 Rb ， 
2) 确定 1) PÆ MHE RES Re HIRA FA 
3) 确定 1) 中 变换 矩阵 SRa 的 欧 拉 参数 。 

A) 利用 斯 坦 利 方法 确定 1) 中 变换 矩阵 SR 的 欧 拉 参数 。 

友 3-51 对 被 旋转 位 置 的 旋转 。 

考虑 与 全 局 坐标 系 G 不 重合 的 位 于 某 一 位 置 的 刚体 B。/A、7 的 方向 余弦 是 : 


0. 5 0. 25 
0. 25 了 一 
cos(1,K) 


cos(j ,J) 
计算 缺 项 ， 并 绕 着 wu 轴 旋 转 45" 之 后 求 刚体 点 卫 在 arp 处 的 全 局 坐标 。 
































1 一 


cos(i,K) 





























We3-52 对 被 旋转 位 置 的 旋转 。 

1) 计算 先 绕 着 z 轴 旋转 30"， 接 着 绕 着 x 轴 旋 转 45 "的 变换 矩阵 。 

2) 假设 在 1) 中 刚体 B 绕 着 > 轴 旋 转 30" 之 后 而 被 给 定 。 确 定 这 时 与 x 轴 重 合 的 旋转 轴 
û, URRE ù 轴 旋 转 45 的 刚体 。 

3) 计算 绕 着 z 轴 旋 转 30"， 接 着 绕 着 x 轴 旋 转 45"， 再 绕 着 > 轴 旋 转 60 "的 变换 矩阵 。 

4) 假设 在 2) 中 在 第 2 旋转 之 后 刚体 B 被 确定 。 确 定 这 时 与 > HA ER ha. UR 
BE ù 轴 旋 转 60" 的 刚体 。 
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刚体 绕 着 轴 oc 旋转 % 角 并 且 移 动 位 移 d， 这 通常 是 位 于 全 局 坐标 系 G 中 刚体 B 的 一 般 
运动 。 刚 体 运 动 可 用 一 个 4X4 和 矩阵 定义 。 


4.1 刚体 运动 








考虑 一 个 具有 局 部 坐标 系 BC(COzyz) 的 刚体 ， 可 在 固定 的 全 局 坐标 系 GC(OXYZ) PÄ 
由 移动 。 刚 体能 够 在 全 局 坐标 系 中 旋转 ， 同 时 刚体 坐标 系 B(COzyz) 的 原点 o 可 相对 地 移动 
到 全 局 坐标 系 G(OXYZ) 的 原点 ， 如 图 4-1 所 示 。 








图 4-1 相对 于 全 局 坐标 系 ， 局 部 坐标 系 的 旋转 和 平移 


如 果 矢 量 64 表示 相对 于 固定 原点 O 的 移动 原点 o 位 置 , 则 局 部 坐标 系 BC(ozyz) 和 全 局 
坐标 系 G(COXY2Z ) 中 刚体 点 P 的 坐标 通过 式 (4-1) 相关 联 : 





crp 一 SRbarp 十 SC (4-1) 
xt (4-1) 中 ， 
Xp Tp Xo 
Srp = Yp Brp = yp |sd =| Y. (4-2) 
Zp Zp Zo 








Hod 被 称 为 B 相对 于 G 的 位 移 或 平 动 ,Re 是 将 3r 变换 成 Sr 的 旋转 矩阵 ， 当 5d =0 
时 。 在 式 (4-1) 中 ， 转 动 和 平 动 的 合成 被 称 为 刚体 运动 。 换 名 话说 ， 相 对 全 局 坐标 系 ， 刚 


CD 


第 4 章 ”运动 的 运动 学 





体位 置 可 用 原点 o 的 位 置 和 刚体 坐标 系 的 定位 来 描述 。 对 于 描述 空间 位 移 来 说 ,将 刚体 运动 
分 解 为 旋转 和 平 动 是 最 简单 的 方法 。 下 面 内 容 说 明了 由 一 个 矢量 所 代表 的 平 动 和 由 前 面 章节 
中 所 给 定 的 任何 方法 描述 的 转动 。 

UE AR : 

图 4-1 所 示 为 在 全 局 坐标 系 中 一 个 被 移动 和 被 转动 过 的 刚体 坐标 系 。 大 部 分 常规 的 旋转 
可 用 罗 德 里 格 斯 旋转 公式 [st (3-90)] 表述 ,， 它 取决 于 Brp。、 刚 体 上 一 点 了 在 刚体 坐标 系 
中 所 测量 的 位 置 矢 量 。 在 平 动 cd& 中 ,刚体 上 的 所 有 点 都 有 相同 的 位 移 ， 因 此 刚体 的 平 动 独 
立 于 局 部 位 置 矢 量 ar。 所 以 ， 刚 体 的 大 部 分 常规 位 移 可 由 式 (4-3) 表述 ， 并 且 有 两 个 独立 
部 分 : 一 个 转动 和 一 个 平 动 。 

Sr =Br cosg + (1—cosd (a * fr dutta xe sing +d 一 Rar od (4-3) 

式 (4-3) 表明 : 刚体 的 大 部 分 常规 位 移 是 绕 着 一 个 轴 的 旋转 和 沿 着 一 个 轴 的 平 动 。 所 
选择 的 参数 点 o 完全 是 任意 的 ,但 是 当 该 点 被 选择 时 ， 刚 体 坐 标 系 就 建立 了 。 转 动 和 平 动 就 
能 唯一 地 被 确定 。 

基于 平 动 和 转动 ， 刚 体位 置 通过 6 个 独立 参数 可 以 被 唯一 确定 : 3 个 平 动 分 量 XL. Yon 
Zo, UR 3 个 旋转 分 量 。 如 果 刚 体 以 旋转 分 量 保持 常量 的 方式 移动 ， 则 该 运动 是 一 个 纯粹 
的 平 动 。 如 果 刚 体 以 平 动 分 量 的 X。、Y。、Z。 保持 常量 的 方式 移动 ， 则 该 运动 是 一 个 纯粹 
的 转动 。 因 此 ， 刚 体 有 3 个 平 动 自由 度 和 3 个 旋转 自由 度 。 

例 4-1 刚体 坐标 系 的 平 动 和 转动 。 

刚体 坐标 系 B(ozyz) 最 初 与 全 局 坐标 系 G(OXYZ) 重合 ， 刚 体 坐 标 系 B(ozyz) HX 
轴 旋 转 45"， 平 移 到 位 置 矢 量 (3 5 7]' 处 。 这 时 在 位 置 矢 量 3r 二 [x y z] WAR, HA 


局 位 置 矢量 为 
0 0 x 3 
0 cos45° in >} (4-4) 
0 一 Sin45” cos45°)\z 7 


=(2+3)f+(0. 707y—0. 707z +5) J +00. 707y +0. 707z +7) K 

Bl 4-2 移动 刚体 坐标 系 。 

图 4-2 所 示 为 在 刚体 坐标 系 B 中 
hE FErp=0.1 +0. 3 jf +0. 3 K AbH 
— 5 P, BARE Z 轴 旋 转 50"， 接 
着 沿 着 X 轴 移 动 一 1 个 单位 ， 沿 着 Y 
轴 移 动 0.5 个 单位 ， 沿 着 Z 轴 移 动 
0. 2 个 单位 。 

在 全 局 坐标 系 中 , 点 P 的 位 置 
矢量 为 






























































Gp =C R ’r 十 sd 一 

















图 4-2 全 局 坐标 系 中 平移 和 旋转 的 刚体 


cos50” 一 Sin50” 0)/(0.1 =j 一 1.166 
Sr 一 Rb8r 十 sd =| sin50 cos50” 0|1|10.3| 十 |0.5| 王 | 0.769 (4-5) 
0 0 LO 3 0.2 0.5 
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Gil 4-3 移动 刚体 的 旋转 。 
刚体 B ES P 有 一 个 初始 的 位 置 矢量 arp 。 





Br 一 (1 2 3)7 (4-6) 

如 果 刚 体 绕 着 x 轴 旋 转 45"， 然 后 平移 到 sd =U 5 6)T7 处 ， 则 点 己 的 最 终 位 置 矢量 为 
1 0 1 1 4 5 

Sr =ER,,45Pr 十 cd =|0 cos45” —sin4d5° |} 2]+]5)=] 4. 23 (4-7) 


0 sin4d5°  cos45° J\3 6 9.53 

TER, Wee EEF od =0 的 假设 。 

例 4-4 机 械 手 臂 的 旋转 及 伸 长 。 

机 械 手 臂 末端 点 Pi 的 位 置 矢量 ， 如 图 4- 3a 所 示 ， 位 于 Srp, =Srp,=(1350 0 900)T 
处 。 机 械 手臂 绕 着 全 局 坐标 系 Z 轴 旋 转 60"， 然 后 伸 长 4 二 720. 2i。 机 械 手 臂 的 最 终 位 置 如 
图 4-3b 所 示 。 

A P 的 位 置 矢量 为 















































Erp, 一 “Rbsrp +°d (4-8) 
这 里 的 SR 一 Rz,60* 是 当 %4d 二 0 时 将 位 置 矢量 rp, 变换 至 rp 的 旋转 变换 矩阵 。 
cos60” 一 sin60” 0 
SR 一 | sin60” cos60” 0 (4-9) 
0 0 1 


Gd 是 刚体 原点 o。 相对 于 全 局 坐标 系 原点 O 的 平移 矢量 。 刚 体 坐 标 系 中 的 平移 矢量 ad 一 
(720.2 0 0)I， 因 此 通过 变换 可 求 得 为 
cos60” 一 Sin60” 0Y/720.2 360. 1 
Sd 一“Rbsd =| sin60” cos60” 0 0 |=|623.71 (4-10) 
0 0 1 0 0 





图 4-3 极 性 RP 机 械 手 
a) 机 械 手 臂 初始 位 置 b) 机 械 手 臂 最 终 位 置 
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因此 ， 机 械 手 末端 的 最 终 全 局 位 置 矢 量 为 
cos60” 一 sin60” 0)/1350 360. 1 1035. 1 
sin60° cos60° 0 0 +1623. 71]=]1792.8) (4-11) 


0 0 1)\ 900 0 900 











Gp =GR B GJ = 
"Pp, =“Rp rp, thd 





例 4-5 变换 的 合成 。 


假设 

2r 一 ?RiIr +7 dy (4-12) 
式 (4-12) 表明 了 刚体 Bi 相对 于 刚体 Bo 的 刚体 运动 。 

cr 一 SR22r 十 cd (4-13) 





式 (4-13) 表明 刚体 Bo 相对 于 坐标 系 G 的 刚体 运动 。 合 成 定义 第 3 个 刚体 运动 ， 该 刚 
体 运动 将 2 的 表达 式 替 换 到 宁 的 方程 中 。 
Sr =R: CR ir 十 241) 十 cd 一 5R22RIr 十 SR22dI 十 sd 一 SRI7 十 cd (4-14) 

因此 ， 有 








GR, =R,?2R, (4-15) 
Sd; =°R2*di +° de (4-16) 
式 (4-16) 表明 从 坐标 系 Bi 到 坐标 系 G 的 变换 可 通过 旋转 矩阵 “RI 和平 动 矢量 qdi 确 定 。 





4.2 章 次 变换 





如 图 全 4 所 示 ， 局 部 坐标 系 B 中 刚体 上 任意 点 了， 可 用 不 同 坐 标 系 中 的 Brs 和 Gr, 表示 。 
矢量 sd 表明 刚体 坐标 系 原点 o 在 全 局 坐标 系 中 的 人 位置。 因此， 在 全 局 坐标 系 GCOXYZ) 中 ， 
刚体 坐标 系 Booryz) 的 常规 运动 是 旋转 运动 YSRe 和 平 动 cd 的 合成 。 











图 4-4 BERR BA GEPA P HRR 








Gr =CRpBr 十 Gd (4-17) 

利用 旋转 矩阵 和 矢量 ， 可 使 我 们 使 用 齐 次 坐标 。 引 入 一 个 新 的 4X4 BRETo, 将 
帮助 我 们 了 解 通过 单一 矩阵 变换 所 表述 的 刚体 运动 。 

oem (4-18) 











这 里 
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和 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





ri Ti rs Xo 
r Too T Yo GRE cd GR cd 
GT 一 21 22 23 -| B j=| B | (4-19) 
Foy Peg) fas Zo 0 0 0 1 0 1 
0 0 0 1 
Xp Tp Xo 
. Y y 、 Yo 
cr 一 | ”| Br=| ?| cad= (4-20) 
Zp Zp Zo 
1 1 1 


齐 次 变换 矩阵 "1 是 一 个 4X4 ERE, CFEM ER A — A ps RE R a — 
标 系 中 。 刚 体 运 动 矩阵 表达 的 延伸 仅仅 是 为 了 简化 数值 计算 。 
RA 个 分 量 的 矢量 乘 以 具有 (n 十 1) 个 分 量 的 矢量 的 表述 被 称 为 齐 次 坐标 表述 。 附 
加 的 元 素 是 一 个 比例 因子 w。 因 此 总 体 上 ， 矢 量 r 一 (x y x)! 的 齐 次 表述 为 






































wa ry 
pare (4-21) 
we r3 
w w 
使 用 齐 次 坐标 说 明 4 个 坐标 的 绝对 值 并 不 重要 。 如 果 w0 且 wo, RME ri/w, 
r2/w 和 rs/z 这 3 个 比率 是 非常 重要 的 ， 因 为 
wa x 
OY |_| (4-22) 
we 之 
w 1 








因此 ， 章 次 矢量 wr 指 的 点 与 > 所 指 的 点 是 一 样 的 。 

如 果 w= 二 1， 则 位 置 矢量 的 齐 次 坐标 与 该 拓 量 的 物理 坐标 是 相同 的 ， 空 间 是 标准 的 欧 几 
里 得 (Euclidean) 空间 。 

自 此 以 后 ， 如 果 没 有 混 消 存 在 ， 并 且 w= 二 1， 则 我 们 将 使 用 规则 的 矢量 及 其 等 效 的 齐 次 表述 。 









































证 明 : 
我 们 将 数字 1 附加 于 一 个 点 的 坐标 中 ， 定 义 的 齐 次 位 置 矢 量 如 下 : 
Xp Tp Xo 
, Y y Yo 
CG; = 和 Brp 一 Cd = (4-23) 
Zp zp Zo 
1 1 1 
依据 齐 次 变换 定义 ， 可 以 求 得 
ad ie ee oe (4-24) 
Xp ru fiz Tfi Xo\(xp Xo Trup Tryp triazp 
Yp = ra 722 Ta Yo ||¥Yp — Yo 十 r2iZp 十 r2syP 十 r23ZP (4-25) 
Zp r3l Taz 133 Lo || zp Lots lp Tz.) p Tr 33% p 
l 0 0 0 1 八 1 1 
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然而 ， 标 准 方法 归纳 于 
Srp= Resrpt sd (4-26) 
Xp ril riz? 113 xp Xo Xu 十 rliZP 十 rl12yP 十 r13 之 P 
Yp |=|ra ree rz | 十 | yp | 二 |Y, |=lYotraaptrayptro32zP (4-27) 
Zp r31 732 133 zp Zo Zotrsixrp Tr32yp r33zP 














式 (4-27) 兼容 齐 次 矢量 定义 和 齐 次 变换 。 
例 4-6 刚体 坐标 系 的 旋转 和 平移 。 
刚体 坐标 系 B(ozyz) 初始 与 全 局 坐标 系 G(OXYZ) BHA. BA X 轴 旋 转 45°, FB 
(3 5 is 这 时 ， 位 于 Br = a, y, TAWA, Hee SERIA 
X 1 0 0 总 
0 cos45° —sin45° 





、 Y 5 
PE spe] |=) i 7 (4-28) 
Z 0 sin45 cos45 7 || >z 
1 0 0 0 VU 


例 4-7 轴 角 的 旋转 和 平移 。 
考虑 具有 单位 长 度 边缘 的 一 立方 刚体 ， 如 图 4-5 所 示 。 如 果 绕 着 w 轴 旋 转 立方 体 
45°, H 

















& 一 (1 1 DT (4-29) 
则 有 
0. 57735 
T a u 
g=— g=—=| 0, 67735 (4-30) 
4 V3 
0. 57735 

















它 的 罗 德 里 格 斯 变换 矩阵 为 : 
0.80474 —0.31062 0.50588 
Ra,s =Icos$ +uut vers tusing=| 0.50588 0.80474 —0.31062] (4-31) 
—0.31062 0.50588 0. 80474 





























图 4-5 具有 单位 长 度 边缘 的 立方 刚体 


105 





D 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





平移 立方 体 的 位 置 矢量 64 为 











cd=0d 1 DT (4-32) 
这 时 ， 有 下 面 的 齐 次 变换 矩阵 : 
0. 80474 —0. 31062 0. 50588 1 
0.50588 0. 80474 —0. 31062 1 
ST g= (4-33) 
—0. 31062 0. 50588 0. 80474 1 
0 0 0 J 
上 表面 各 顶点 的 位 置 矢 量 为 
0 1 
Bre=|0|,2 rr=|0], 
1 iL 
1 0 
Bro =|1),2rH=]1 (4-34) 
1 1 
因此 ， 利 用 sr =CTp®r ， 旋 转 之 后 的 顶点 全 局 坐标 系 的 位 置 矢 量 为 
1.505 2. 310 2 1.196 
Cre =/|0. 689 |.% rp=|1.196|, re=]|2|, ra =] 1. 495 (4-35) 
1. 804 1.495 2 2. 31 


例 4-8 REFER AR TRIE PES Ty 分 解 成 平 动 和 旋转 。 
齐 次 变换 矩阵 STs 可 以 分 解 为 纯粹 的 旋转 矩阵 5Ra 和 纯粹 的 平 动 矩阵 5Ds 的 乘积 。 

















1 0 0 Xo)(ri riz ris 0 ri riz ris Xo 
、 oy as 0 1 0 Yo jlrz ree rz 0 ra rz rz Y, 
ST3=°DpoRa = (4-36) 
0 0 1 Zo |lrsl r32 r33 0 r31 T32 733 o 
0 0 0 1 0 0 0 1 0 0 0 1 
换 名 话说 ， 先 纯粹 的 旋转 ， 后 纯粹 的 平 动 ， 即 可 实现 变换 。 
注意 ， 将 变换 矩阵 分 解 为 平 动 矩 阵 和 旋转 矩阵 ， 是 不 可 交换 的 。 
CSTg=°DgB Rg ÆA Rg. Dg (4-37) 
PRIM. ADE SFE REST, 和 旋转 矩阵 “Ra 的 定义 ， 有 
5TB 王 5DBcRa 王 DB 十 <RB 一 I 一 “Ra 十 “DB 一 工 (4-38) 
例 4-9 纯 平 动 。 
刚体 坐标 系 B (ozyz) 初始 与 全 局 坐标 系 G (OXYZ) 重合 ,平移 到 : 
dx 
Sd =|dy (4-39) 
dz 
这 时 ， 刚 体 的 运动 是 一 个 纯粹 的 平 动 ， 全 局 坐标 系 中 刚体 上 一 点 的 变换 矩阵 为 
X 1 0 0 dx)\fx 
Gr =CT,B r 一 GD5Br 一 Y = oD ee |e (4-40) 
Z. 0 0 1 dzilz 
1 0 0 0 1 1 
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例 4-10 绕 着 全 局 坐标 系 轴 旋 转 的 齐 次 变换 。 
利用 绕 着 全 局 坐标 系 轴 旋 转 的 齐 次 表达 式 ， 绕 着 Z 轴 旋 转 的 变换 矩阵 为 


cose —sing 0 


GT =Rz., = (4-41) 


0 0 1 
0 0 0 
例 4-11 绕 着 全 局 坐标 系 轴 的 旋转 和 沿 着 全 局 轴 的 平 动 。 
点 卫 在 刚体 坐标 系 中 的 位 置 是 (0，0，200)。 如 果 刚 体 绕 着 全 局 X 轴 旋 转 30°, 
且 刚 体 坐 标 系 原点 平移 到 (X,，Y，2Z)=(500，0，600)， 则 该 点 在 全 局 坐标 系 的 位 置 
矢量 为 


0 
sine cosa 0 0 
0 
ik 
































1 0 0 500\/(1 0 0 0\( 0 500 
0 1 0 0 |lo cos30° —sin30° 0|| 0 —100 

one : = (4-42) 
0 0 1 600|f0 sin30°  cos30° 0||200| |773.2 
000 1)bo o 0 1 八 1 1 


例 4-12 绕 着 局 部 坐标 系 轴 的 旋转 和 沿 着 局 部 坐标 系 轴 的 平 动 。 
KP 在 刚体 坐标 系 中 的 位 置 是 (0，0，200)。 如 果 刚 体 坐 标 系 原点 平移 到 (X, Y, 
Z) 二 (500，0，600)， 接 着 绕 着 局 部 坐标 系 中 x 轴 旋 转 30"， 则 该 点 在 全 局 坐标 系 的 位 置 矢 





























1 0 0 500)\/1 0 0 0 0 500 
0 1 0 0 |0 cos30° —sin30° 0|| 0 一 100 
nie i = (4-43) 
0 0 1 600/0 sin30 cos30 0 || 200 Claez 
0 0 0 1 0 0 0 1 1 1 
例 4-13 平移。 
如 果 刚 体 点 的 位 置 矢量 为 
Br 一 (一 1 0 2 DF (4-44) 
平移 后 的 位 置 矢量 为 
Sr 一 (0 10 一 5 1)7T (4-45) 
这 时 ， 有 如 下 变换 方程 : 
0 1 0 0 dxf 一 1 
10 0 1 0 dy 0 
= (4-46) 
= 5 0 0 1 dz 2 
1 0 0 0 1 l 
因此 ， 有 
一 1 十 dx 一 0,dy 王 10,2 十 dz 一 一 5 (4-47) 
即 
dx=l1.,dy=10.dz 7 (4-48) 





例 4-14 纯 平 动 和 纯 旋转 。 
下 面 给 出 沿 着 X、Y、2 轴 平 动 和 绕 着 X、Y、2 轴 旋 转 的 一 组 基本 齐 次 变换 : 
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ws: 运动 学 、 动 力学 与 控制 技术 








10 0 a 
、 0 1 0 0 
oTp=Dx,,= ey 4. 4 (4-49) 
0 0 0 1 
1 0 0 0 
rek = [|O cosy —siny 0 (4-50) 
P ag 0 siny cosy 0 
0 0 0 1 
1 0 0 0 
、 0 1 0 6 
Tp=Dy,,= b oad o (4-51) 
0 0 0 1 
cosB 0 一 Sing 0 
、 0 1 0 0 
GTs=Ry,g= | (4-52) 
sing 0 cosp 0 
0 0 0 1 
1 0 0 0 
: 0 1 0 0 
CTg=Dz.. = a (4-53) 
č 
0 0 0 0 
cosa —sine 0 0 
sina cosa 0 0 
oTp=Rz,,= j rs (4-54) 
0 0 1 


例 4-15 齐 次 变换 可 作为 一 个 矢量 加 法 。 
由 图 4-4 可 知 ， 点 P 的 位 置 可 用 矢量 加 法 描述 : 
Srp 一 Gd 十 Brp (4-55) 
因为 矢量 方程 具有 普遍 意义 ， 当 所 有 矢量 在 同一 坐标 系 中 被 描述 时 ， 我 们 需要 将 arp 变 
换 到 G 坐标 系 中 ， 或 者 将 5d 变换 到 B 坐标 系 中 。 因 此 ， 被 应 用 的 矢量 方程 为 
































crp 一 CRBB rp +°d (4-56) 
或 者 
BRoo rp = Rood 十 Brp (4-57) 
式 (4-56) 定义 了 从 B 到 G 的 一 个 齐 次 变换 : 
Gyrp 一 GTPBB rp (4-58) 
f [7 Ka 
CTg = (4-59) 
0 1 
式 (4-57) 定义 了 从 G 到 了 的 一 个 齐 次 变换 : 
Brp 一 BToGrp (4-60) 
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B R =B cG GRT —GRTG 
ro-| ee ")-{ — ‘| (4-61) 
0 i 0 1 

KG 4-16 ”无穷 远 处 的 点 。 
无 穷 远 处 的 点 有 一 个 具有 章 次 坐标 的 便捷 表述 。 考 虑 作为 一 点 第 4 个 坐标 的 比例 因子 


w， 因 此 点 的 齐 次 表述 可 用 式 (4-62) 给 出 。 





x x/w 

y |_jo/w (4-62) 
之 z/w 

w 1 


当 w>0 时 ， 点 则 趋 于 无 穷 远 处 。 我 们 可 以 改变 规则 ， 即 用 下 面 的 齐 次 坐标 表述 无 穷 远 
处 一 个 点 。 





(4-63) 


更 重要 的 是 ， 无 穷 远 处 的 一 点 表示 一 个 方向 。 在 这 种 情况 下 ， 它 说 明 所 有 直线 平行 于 矢 
量 r 二 [x y > 并， 它们 相交 于 无 穷 远 处 的 一 个 点 。 
在 无 穷 远 处 ， 点 的 齐 次 坐标 变换 引入 齐 次 变换 矩阵 的 一 个 齐 分 解 。 





[fini 
8 





STp=| ~ - (4-64) 


0 0 0 1 
若 前 3 列 的 第 4 个 坐标 为 零 ， 则 它们 便 代 表 无 穷 远 处 的 点 ， 此 时 该 点 是 相当 于 3 个 坐标 
轴 的 方向 。 第 4 列 的 第 4 个 坐标 是 1， 代表 坐 标 系 的 原点 位 置 。 
友 例 4-17 广义 齐 次 变换 。 
A (4-19) 中 的 齐 次 变换 是 常规 齐 次 变换 的 一 种 特殊 情况 。 广 义 齐 次 变换 已 经 广泛 应 用 
于 计算 机 图 形 领域 之 中 了 ， 它 采取 如 下 形式 : 
Po Bod 
Ts = 
pl(1X3) w@X1) 
对 于 机 器 人 学 来 说 ， 我 们 总 是 将 变换 矩阵 [全 ] 中 的 最 后 一 行 矢 量 置 为 [0 0 0]。 然 而 ， 
更 为 一 般 的 形式 、 如 式 (4-65) 是 非常 有 用 的 。 例 如 ， 当 图 形 模拟 器 或 者 可 视 系 统 被 加 入 到 
综合 机 器 人 系统 时 。 
左上 方 的 3X3 FRR Ra 表示 移动 坐标 系 B 相对 于 参考 坐标 系 A 的 方向 。 右 上 方 的 
3 久 1 子 和 矩阵 ^d 表示 移动 坐标 系 B 的 原点 在 参考 坐标 系 A 中 的 位 置 。 左 下 方 1X 3 FEE p 
表示 视角 变换 ， 右 下 方 元 素 w 是 一 个 比例 因子 。 





(4-65) 














4.3 逆 齐 次 变换 








齐 次 变换 矩阵 具有 简化 计算 的 优点 ， 但 是 却 失 去 了 正 交 特性 。 如 果 我 们 用 式 (4-66) 
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ws: 运动 学 、 动 力学 与 控制 技术 








表示 STs， 
、 I Cd) Rg 0VfGRB Sd 
GT 二 (4-66) 
0 1 0 1 0 1 
则 有 
DOSA CRpg Gd —1 CRE —CRIGg 
BTo 一 GT 一 = (4-67) 
0 1 0 1 
式 (4-67) 表明 : 
ST, 'STR=L (4-68) 
PRIN, AEFR IE REDIF AS ETE CY), Cw Fa EIGN SE TE Ye EE, 
OTs ÆET} (4-69) 


UE AA 1: 

图 4-6 所 示 为 相对 全 局 坐标 系 G (OXYZ) 的 被 旋转 和 被 平移 的 刚体 坐标 系 B 
(Cozyz)。 将 点 卫 的 坐标 从 全 局 坐标 系 转换 到 刚体 坐标 系 的 变换 是 Tec， 它 是 变换 拢 
MEST AY ii HE BF 

















K 4-6 刚体 坐标 系 B (ozyz) 相对 全 局 坐标 系 G (OXYZ) 的 旋转 和 平移 
让 我 们 从 Sr 表达 式 和 将 8r 变换 成 Sr 的 变换 矩阵 5Ta 定义 开始 。 
=" Regi Parra (4-70) 
Gr GRE Gd By 
hee idle re 
并 且 求 得 
Br =° Rp (Sr ~—d)=°RBr —°RESd (4-72) 
以 便 表 达 将 Sr 变换 成 ar 的 变换 矩阵 ?8T c 。 
re 5t ad (4-73) 
0 1 
证 明 2: 
用 几何 表达 式 也 可 求 得 STGc。 利 用 旋转 矩阵 的 逆 和 矩阵 ， 有 
CRZ! =R] =T6 (4-74) 


和 述 刚体 坐标 系 中 sd 的 逆向 变换 ， 说 明 相 对 于 刚体 坐标 系 原 点 的 全 局 坐标 系 原点 。 
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Bd 一 BRccd 一 GRIGd 
式 (4-75) 允许 我 们 定义 齐 次 变换 矩阵 BTc 。 


BRG —Bd 
BT = = 
0 1 


GTG 一 GTT 1 


我 们 使 用 标记 号 


GRE 


(4-76) 


(4-77) 


并 且 记 住 依据 式 (4-67) 计算 齐 次 变换 矩阵 的 逆 和 矩阵， 并 不 需要 通过 4X4 BEY e 





例 4-18 章 次 变换 矩阵 的 首 矩 阵 。 


假设 





0.643 一 0.766 


0.766 0.643 
= 
0 0 
0 0 
则 有 
0. 643 
SRB 一 |0.766 
0 
因此 
GRT —GRicg 
BTg=¢T,;!=| ° Be 
0 1 





例 4-19 变换 矩阵 和 点 的 坐标 。 


基于 固定 刚体 中 特定 点 的 位 移 ， 有 时 有 可 能 甚至 很 方便 地 描述 刚体 的 运动 。 

假设 A、B、C 和 D 是 具有 下 列 坐 标的 两 个 不 同位 置 的 4 个 点 : 
Ai(2,4,1).B1(2,6,1).C1(1,5,1) .D1(3,5,2) 
Az(5.1,1).B2(7,1,.1).C2(6.2,.1) .D2(6.2,3) 

必须 要 有 一 个 变换 矩阵， 将 初始 位 置 变 换 到 终端 : 





2 2 1 3 


pi- pa a 
= =e Q 
= e Oo 


5 
2 
1 


因此 ， 有 


规则 。 这 个 标记 与 工 符 阵 乘 以 道 矩 阵 T ! 是 一 致 的 ， 因 为 : 
cT51cTB 一 工 


0. 643 
一 0.766 0. 643 
0 
0 


7 
1 
1 
1 


= =e N Q 
= 3 BY Dm 


(4-78) 


(4-79) 


(4-80) 


(4-81) 


(4-82) 


(4-83) 
(4-84) 


(4-85) 
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则 


X 


6 
2 
1 
1 


=H wW by m 


O a o OFF e CI 
Yi OS, ja Se E 








=l 


= =e A N 
= =e Q N 
= e n e 


3 
5 
2 
1 


= S| æ Oo 
= e e N 
= e N Q 


0 
1 
| 2 
0 1 
例 4-20 ”快速 着 变换 。 

















对 于 数值 计算 ， 更 为 实际 的 是 利 
考虑 一 个 变换 矩阵 ， 
rll riz r3 ria 1 0 0 ru 
r21 r22 r23 r24 0 1 0 T 24 
T= =DR= 
r31 732 133 134 0 0 1 ra 
0 0 0 1 0 0 0 1 
因此 
rll riz? 113 
T1=(DR)'=R-'D-1=Rt™D 一 | 7 > 
r31 T32 1 33 
0 0 0 
ri rT12 r13 T11714 T21724 T31734 
[rar 22 rag T12714 122124 1327 34 
r31 732 733 T13714 723724 733734 
0 0 0 1 


KB 4-21 一 般 齐 次 变换 矩阵 的 首 矩 阵 。 








ri 


re O CO CO 
OOO H 


r12 


D 2 — © 


oF O O 


7/2 
一 3/2 
1/2 
=3/2 


H å O O © 


BRIEKE T EAEE, TE 4 SIE AL BY OC 和 D 所 构成 


» WARE RA AY DAJE EY 


É B 
T= 
C D 


| 





—E™'!CA7! 


r-(" !+A~!BE~!CA 


这 里 


1 —A-1iBE-! 


E ! 


E=D-—CA !B 


对 于 最 一 般 的 齐 次 变换 情况 ， 有 


则 有 
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ar, {RsC3X3) 
? L pax3) 


T=^Tp 


Ad (3X1) 
w(1X1) 


(4-86) 


用 矩阵 乘法 的 优点 将 变换 矩阵 转换 成 平 动 乘 以 转动 。 


(4-87) 


(4-88) 


(4-89) 


(4-90) 


(4-91) 


(4-92) 


(4-93) 








A=^Rpg 
B=^d 
C=p=(pi pz ps) 
D=wix; 5w 
因此 ， 有 
E =Eix1=E=w—(pi pz p3)4Rg'4d=w—(pi pz p3)4Rghd 





=1—p\(dirutderatd3r3)+ps(diriz t+ doro2 td 3r32) | P3(dirizg+do2r23+d3r33) 


1 
F194 21 = = 5 
E CA E's! £2 


其 中 ， gı = Piru + porte + pri, ge =p 
P2r32 T P3rss 。 





diriu 





1 
ZA BE = d 1r12 





d 1r13 








A i+A BE CA 1!=F=|f2 


b 
HE 





fn=rut (pirit tpr tpar) ld 





fe5Sra tp r2 por22 + p3r23) (dir 





fis=ratap f3 por 32 P3r33)(d r 





fare tp rii 十 轧 27rl1l2 十 力 37r13)(CC17r 





fo2=ro2 Tp rz p2r22+ p3r23) (dir 





fas=r32T pP r3 P2r32 Pp3r33)(d T 





fa=rist Ee rı Por. P3ri3s)(d r 





f32=r23 FP re por 22 p3r23)(d T 




















f33=7r33 F= p r3 P2r32 p3r33)(d T 


对 于 坐标 齐 次 变换 的 情况 来 说 ， 有 
T=4Ts, 
A =ARp 















































£3) 
ra 十 Porz 十 旋 3723， 
d2r21 Td3r3l 
d 27 22 d 37 32 
d2r23 Td3733 
fu fre fas 
fz22 fos 
fa fsz f33 
rutqdzrzltdsr31) 
1 darz drz) 
ı Fd2rz21 Fd3r31) 
2 十 da2722 十 Ca37r32) 
2 十 da2722 十 Ca37r32) 
2 d2r22 +d3r32) 
3 十 Ca2723 十 Ca37r33) 
3 十 Ca2723 十 Ca37r33) 
3 Td2r23 ds3r33) 


(4-98) 


(4-99) 


g3 = pira 十 


(4-100) 


(4-101) 


(4-102) 


(4-103) 
(4-104) 
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B=d (4-105) 

c=(0 0 0) (4-106) 

p=(1) (4-107) 
则 有 

E=(1) (4-108) 
因此 ， 有 

‘ra 0 | (4-109) 
0 1 


4.4 BAKU EM 


分 析 复 合 变换 的 3 个 坐标 系 如 图 4-7 所 示 。 








图 4-7 分 析 复 合 变换 的 3 个 坐标 系 





将 坐标 从 BB 坐标 系 转换 到 A 坐标 系 ， 以 及 由 C 坐标 系 转换 到 B 坐标 系 的 变换 矩 





AR Ad 
‘m= ') (4-110) 
0 
BRc Bd? 
BTc 一 (4-111) 
0 1 


因此 ， 从 C 坐标 系 转换 到 A 坐标 系 的 变换 矩阵 为 
AR» a. alae We 


ATc =ATp®Tc = 
0 1 0 1 0 1 


(4-112) 
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cp -| Ré TARE 一 BRLARECARBB d: +^ a Wa —BREB d,—ARGA *) 
A= = 


0 1 0 1 
ARE —ARGA d> 
4 0 1 ) 
(4-113) 
当 几 个 位 移 连 续 出 现时 ， 章 次 坐标 值 受 到 了 人 们 的 青睐 。 齐 次 坐标 变换 可 写 为 
Peery is Tals (4-114) 
而 不 是 





GR, rp 十 Gd 一 GRITIR2[2R3 CR rp td) td J+ dz} + od: (4-115) 
Bil 4-22 多 坐标 系 的 齐 次 变换 。 
图 4-8 所 示 为 局 部 坐标 系 Bo (x2zy2z2) 中 的 一 个 点 P。 利 用 齐 次 变换 矩阵 求 得 全 局 从 
mA G (OXYZ) PK P 的 坐标 。 
2rp 表 示 局 部 坐标 系 Bo (zsyyz?) PA P 的 位 置 。 因 此 ， 它 在 坐标 系 Bi Criyiz1) 
中 的 位 置 矢 量 为 











(4-116) 
Al 4-8 局 部 坐标 系 Bo (zayzz?) 中 的 点 了 

因此 ， 它 在 全 局 坐标 系 G (OXYZ) 中 的 位 置 矢量 为 

xX zı x2 x2 

Y ie ba yı Di ate 了 | y2 Pa | y2 

Z 0 il Zi 0 1 0 1 之 2 0 iL 之 2 

1 1 1 1 
(4-117) 


KG 4-23 多 坐标 系 的 齐 次 变换 。 

齐 次 变换 矩阵 能 够 用 来 描述 绕 着 经 过 某 一 非 原 点 轴 的 旋转 。 图 4-9 LP SE ee oo 旋转 
p 角 ， 并 且 该 轴 是 通过 非 原点 的 一 个 点 了 的 轴 。 

设 一 个 位 于 点 卫 处 的 局 部 坐标 系 也 平行 于 全 局 坐标 系 G。 这 时 ， 绕 着 轴 的 旋转 可 以 
表述 为 沿 着 一 d 平 动 ， 使 刚体 坐标 系 B 进入 全 局 坐标 系 G， 接 着 绕 着 轴 之 旋转 ， 并 且 沿 着 d 
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进行 逆向 平 动 。 
、 I dR OYI —d Ri,s d—Ri,sd 
STge=D å, aR, Då, -a = = (4-118) 
0 IL oO 1 人 0 1 0 1 
式 中 ， 
uj vers cy ul uzversPé—uUu3sShd uiusversg Tu2sy 
Rz, =| uiuzversg Fuss us vers cy u2u3versd — uisg (4-119) 
ujugversdé—u2zsd uzu3versġ T us$ u? vers +c¢ 











dı (1—uj )vers¢ —u vers (d2u2+d3u3) tsġ(d2uz3—dzu2) 
d—Ri.sd =| d2(1—u3 )versé —u2versd (d3u3 tdiui) +sd(d3u1 —dius) (4-120) 
d3(1—u )versé —u3 vers (di u1 d2u2)+s¢(diu2z—d2u1 ) 




















图 4-9 绕 着 未 通过 原点 的 轴 旋 转 


KG 4-24 旋转 圆柱 体 。 
想象 一 个 圆柱 体 ， 它 的 半径 R= 二 2， 其 轴 忆 位 于 qd 处 。 


0 2 
& 一 |0| d=|0 (4-121) 
1 0 


如 果 圆 柱 体 绕 着 其 轴 旋 转 90"， 圆 柱 体 边 缘 上 的 每 点 移动 90"， 并且 位 于 围绕 着 平行 于 
(2. y) 平面 的 一 个 加 上 。 该 运动 的 变换 矩阵 为 


I DRR O 一 4 
ST, =D f, aRi, ¿D å, -a 5 




















o ] 作 0 lo 1 
Loo Bes Ee ote oe Se 
_{o 1 0 off i o off? Fo a 
0 0 1 0 2 2 0 0 1 0 a 
0 0 0 1) 0 0 1 Ollo o o 1 
0 0 0 1 
¢ =i oO 2 
ahh 0 0 一 ? 
Jo o 1 0 
0 0 © 1 
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考虑 圆柱 体 上 位 于 原点 的 一 点 。 在 旋转 之 后 ， 则 能 看 见 该 etal 





0 —1 0 2 \(0 


i RE 0 0 一 2|0 
Gr 一 GTpGr = 
0 0 1 0 0 
0 0 © 1 1 


例 4-25 在 全 局 坐标 系 中 RPR 机 器 人 的 末端 执行 器 。 





(4-123) 


A P 是 如 图 4-10 所 示 的 机 器 人 最 后 一 个 机 械 臂 末端 的 点 。 坐 标 系 Bo (zyyzz2?) 中 点 
PWR rp. MRA Bo (zyzz?) RA z2: HEH. WA yi WED., BRAR Bi 





(zx1y1z1) 绕 着 全 局 坐标 系 G (OXYZ) HZ 轴 旋 转 ， 其 原点 位 于 S41 处 。 这 


标 系 G (OXYZ) PA P 的 位 置 矢量 为 





图 4-10 RPR 机械 手 


Gr =ER R2? rp +R! dz +° di=°TiT, rp 一 CT22 r 


式 中 ， 


四 了 
lT,= 
0 1 


、 SR1IR CRi'd2+%d, 
CT, 一 


1 
il 4-26 全 局 坐标 系 中 SCARA 机 器 人 未 端 执 行 器 。 


时 ， 在 全 局 坐 


(4-124) 


(4-125) 


(4-126) 


(4-127) 


图 4-11 所 示 为 具有 全 局 坐标 系 G (OXYZ) 的 一 个 R1RIP 机 器 人 ， 该 全 局 坐标 系 G 
(OXYZ) 附着 在 基 座 上 ， 坐 标 系 Bi (xiy1z1) MERR Bo (zsyzz?) 分 别 附 着 在 关节 1 


和 关节 3 上 。 
将 坐标 系 B 中 的 点 变换 至 全 局 坐标 系 G 中 的 变换 和 矩阵 为 
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cos01 —sind, 
sind coSO1 
G 
B1 
0 0 
0 0 





图 4-11 例 4-26 中 的 SC 





0 LicosOl 
0 Lisin01 
1 0 
0 1 








ARA 机 器 人 


将 坐标 系 Bo 中 的 点 变换 至 坐标 系 Bi 中 的 变换 矩阵 为 


cos0» 一 Sin0， 
sind» cosd» 
Bı Tb， 一 6 4 
0 0 


Ll2cOsO» 


—h 


0 
0 712sin0, 
1 
0 1 





因此 ， 将 末端 执行 器 Bo 中 的 点 变换 至 全 局 坐标 系 G 中 的 变换 矩阵 为 


cosO1 一 Sin01 0 /icosOl cos0» 一 Sin0， 


sing | cos 
0 0 
0 0 


CTh =C0T Tg; = 0 


1 








sin(@, +02) cos(@; +42) 
0 0 
0 0 


0 
1 
0 
cos(@; +42) sin(@; +62) 0 licos0; +l2cos(O, +42) 
0 
1 
0 


0 0 


0 
Lı sinO, sinfz cos, 0 
1 
0 


0 0 





Lisin0l 十 /2Ssin(CO1 +@2) 


=h 
1 


(4-128) 


(4-129) 


Lo cos? 


Z2sin@2 


=h 


1 


(4-130) 


最 后 一 个 坐标 系 的 坐标 原点 os 在 其 局 部 坐标 系 中 的 位 置 矢量 为 中 ro = 

(0 0 0 DT, KE, ES o: 在 全 局 坐标 系 中 的 位 置 拓 量 为 
LicosO1 十 /cos(O1 +02) 
/isin01 十 /2sin(O1 +02) 


Sr 一 GTpb: ro: = 
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—h 
1 


(4-131) 





例 4-27 对 象 操作 。 

可 用 包含 在 局 部 坐标 系 中 描述 刚体 一 些 具体 点 的 齐 次 坐标 表述 刚体 几何 。 具 体 点 通常 是 
顶点 。 

4-12a 为 一 个 横 形 块 配置 。 在 刚体 坐标 系 B 中 ， 模 形 块 的 顶点 坐标 被 集中 在 矩阵 P 
ms 























1 1 1 0 0 0 
0 0 3 3 0 0 
Bp= (4-132) 
4 0 0 0 0 4 
1 1 1 1 1 1 


BERRA Z 轴 旋 转 一 90"， 沿 着 X 轴 平 移 3 个 单位 ， 如 图 4-12b 所 示 。 通 过 将 相应 变 
换 和 矩阵 乘 以 矩阵 P 便 可 求 得 全 局 坐标 系 G 中 顶点 的 新 坐标 。 





1 0 0 3\(cos(—90°) —sin(—90°) 0 0 0 103 
0 1 0 Of] sin(—90°) cos(—90°) 0 0 —-1000 
STp=Dx,3Rz, 90 = 一 (4-133) 
0010 0 0 1 0 0 010 
0001 0 0 0 1 0 001 
因此 ， 顶 点 1 一 顶点 6 在 旋转 之 后 ， 其 全 局 坐标 被 集中 在 下 面 的 矩阵 中 : 
© 10 3\f1 110 0 0 3 3 6 6 0 3 
、 一 1]000l0033.00 一 “一 下 一 0 
Gp—c7,2p— = (4-134) 
0 0O 10400004 4 0 0 00 4 
0 00 IJU 1 1 1 1 1 1 1 1 14111 











a) b) 


图 412 依据 刚体 中 的 一 些 点 描述 刚体 运动 





例 4-28 圆柱 坐标 。 

有 一 种 情况 是 ， 我 们 希望 确定 圆柱 坐标 中 机 器 人 末端 执行 器 的 位 置 。 通 过 沿 着 X 轴 平 
Br 个 单位 ， 接 着 绕 着 Z MIER o 角 ， 最 后 沿 着 Z 轴 平 移 > 个 单位 ， 可 得 一 组 圆柱 坐标 ， 
如 图 4-13 所 示 。 
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图 4-13 例 4-28 中 点 P 的 圆柱 坐标 








因此 ， 从 圆柱 坐标 系 C (Orez) 到 笛 卡 儿 坐 标 系 G (OXYZ) 的 齐 次 变换 矩阵 为 








1 0 0 O)\(cosp 一 Sinp 0 O)fl 0 0 r 
、 0 1 0 Ollsing cose 0 .00 1 0 0 
ee i | i a |- o 10llo 0 1 0 
0 0 0 1 0 0 0 1/\O0 © 0 1 
. (4-135) 
cosp —sing 0 rcose 
__|sing coso rsing 
fo 0 1 z 
0 0 
作为 一 个 示例 ， 考 虑 圆柱 坐标 系 中 位 于 | 1， T, zeA P, MA P HEMRA 
标 系 中 的 位 置 矢量 为 
cos 本 sin > 0 cos 本 |70 0.5 
T T x ||O 0. 866 
sin 本 cos y 0 sin r = ee 
0 0 1 2 1 1.0 
0 0 0 1 








例 4-29 BRA AR 

先 沿 着 Z 轴 移 动 r 个 单位 ， 接 着 绕 着 Y 轴 
旋转 9 角 ， 最 后 绕 着 Z 轴 旋 转 wp 角 ， 可 得 一 组 
球 坐 标 ， 如 图 4-14 所 示 。 

因此 ， 从 球 坐 标 系 S (Orbe) 到 笛 卡 儿 坐 
标 系 G (OXYZ) 的 齐 次 变换 矩阵 为 








图 4-14 例 4-29 中 点 PP 的 球 坐 标 


cosOcosp 一 Sinp cosgsin@d rsin@cose 


cosOsinp cosp sin0song 7rsin0sinp 


STs=Rz,oRy,0Dz,, = (4-136) 


— sind 0 cos? recos 


0 0 0 1 
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这 里 
cosp 一 Sinp 0 0 
si O 0 0 
Rre=| P Sp (4-137) 
0 0 1 0 
0 0 0 1 
cos0 0 sind 0 
Ry „= g : i i (4-138) 
s 一 Sin0 0 cosO 0 
0 0 0 1 
1 0 0 0 
0 1 0 0 
Dz r— (4-139) 
0 0 1 r 
0 0 0 1 
作为 一 个 示例 ， 考 虑 球 坐 标 系 中 位 于 [2， EE S e—a P. AP 在 笛 卡 儿 坐 标 系 
中 的 位 置 矢量 为 
T T é ot 人 T. T 
cos -cos cos -ysin cos ysin cos ysin = 
3 3 3 3 3 3 3 0 0. 866 
as oe: Do ea ID LS 
cos sin 3 cos 3 sin ysin 3 sin ysin 3 = è TETT 
0 1.0 
=a 0 eee 2cos — 1 1.0 
sin 3 3 83 F 
0 0 0 1 
妈 4.5 螺旋 坐标 
任何 刚体 运动 都 可 以 由 沿 着 某 轴 的 单一 平 动 和 绕 着 该 轴 的 单一 旋转 运动 合成 产生 。 这 称 





为 沙 勒 定理 (Chasles theorem)。 这 样 的 运动 被 称 为 螺旋 运动 。 考 虑 如 





图 4-15 所 示 的 螺旋 运 





Zo A P Se uo 螺旋 轴 旋 转 ， 同 时 沿 着 a FY 
动 ， 与 此 同时 离开 该 轴 沿 着 螺旋 结构 运动 。 

刚体 绕 着 螺旋 轴 的 角度 旋转 被 称 为 扭转 。 
ME h 与 旋转 角 Y% 的 比值 。 








yah 
$ 


换 名 话说， 刚体 平行 于 其 螺旋 轴 移 动 一 个 单位 转角 所 经 
过 的 直线 距离 称 为 螺 距 。 如 果 p 二 0， 则 螺旋 是 








W, WR 2 过 0， 则 它 是 符合 左手 定 则 的 。 
螺旋 可 上 日 
CRA IR 

















HE Ritu, Pi Kins 、 扭 转角 6 ME h 
Ep) 表示 为 h, 6. ú, s), MERES 表示 点 


FF 动 。 因 此 ， 螺 旋 轴 上 的 任何 点 沿 着 螺旋 轴 平 





IRIE p 是 平 动 


(4-141) 


符合 右手 定 则 





绕 着 直线 转动 并 沿 着 


图 4-15 














在 螺旋 轴 上 的 全 局 位 置 。 扭 转角 $、 旋 转轴 u MIRE p 称 为 








该 直线 移动 的 螺旋 运动 
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螺旋 参数 。 

螺旋 变换 的 齐 次 和 矩阵 表述 是 绕 着 螺旋 轴 旋 转 和 平 动 的 合成 。 如 果 “ 通过 了 坐标 系 的 原 
点 ， 则 = 一 0 时 的 螺旋 运动 被 称 为 中 心 螺旋 (hh，$，w)。 

螺旋 是 描述 刚体 运动 的 另 一 基本 变换 方法 。 在 机 器 人 应 用 中 ， 绕 着 一 轴 旋 转角 位 移 
Ra,s 并 沿 着 此 轴 平移 所 合成 的 线 位移 Di 重复 性 地 不 断 增 大 。 

对 于 中 心 螺旋 运动 ， 有 




















Gsp(h,$ ú) =Da,Ri,s (4-142) 
式 (4-142) 中 ， 
1 0 0 hu 
0 1 0 hue 
Din = (4-143) 
0 0 0 1 
ulvers$+cg ujuzversd—uzsp uiu3aversg Tus 0 
2 
ulu2 Vers$ TU3S usVvers$ Tc WUW2U3VerS 由 一 MI1S 0 
pea Oe eee ae (4-144) 
ujuzversd—Uu2Sd U2U3VersdTU1S¢d usversg cy 0 
0 0 0 1 
因此 ， 
ulivers$ +c uj uz versd—u3shd uiusversg Tu2ssg hui 
2 
: P ui U2 vers$ Fuss u5 versd +c u2u3versé—uish hu 
SCT tO iia Maas ii Me Hg E re 
uj u3versd—u2Shd uus3versp Tuisyg us versġ tcg hus 
0 0 0 1 





作为 一 个 特殊 情况 的 结果 ， 中 心 螺旋 变换 矩阵 包括 纯 的 或 者 基本 的 平 动 和 转动 ， 因 为 纯 
粹 的 平 动 相 当 于 $= 二 0; 纯粹 的 转动 相当 于 有 二 0 (或 者 p= 二 2)。 反 转 中 心 螺旋 可 以 定义 为 
Ss (—h, —$, ú), 

对 于 一 般 螺 旋 运 动 ， 有 











l da - Di | 四 了 
GTB=°sp(h,$ ,us ) 一 一 (4-146) 
0 1 0 1 
式 (4-146) 中 ， 
SR a=Icosgé +uu! (1—cos¢) +u sing (4-147) 
Sd =[ A úT) (1 — cosp) —wsing Js thu (4-148) 
当 螺 旋 运 动 是 非 中 心 螺旋 运动 ， 并 且 ww 并 没有 通过 原点 时 ， 从 PP 移动 至 P" 的 螺旋 运动 
可 被 定义 如 下 : 
有 一 (P —s cosg + (1—cosd) Lai + Gp —s ) Jui +Lu X (p —s ) sing +s 十 7 (4-149) 
或 者 ， 
p”=°Rg(p —s )+s 十 1 一 GRB p +s 一 GRBS hu (4-150) 
因此 ， 有 
p'=s(hsd.u-.s )p =T p (4-151) 


这 里 的 工 是 式 (4-146) 的 齐 次 变换 和 矩阵。 
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Dy 
in 


& fl 


Cs 被 称 为 定位 矢量 ， 它 是 刚体 坐标 在 螺旋 运动 之 前 的 全 局 位 置 矢 量 。 矢 量 pp" FR 
是 点 了 在 螺旋 之 后 和 之 前 的 全 局 位 置 矢 量 ， 如 图 4-16 所 示 。 


























[fini 





itp 分 





图 4-16 刚体 螺旋 运动 


螺旋 轴 用 单位 矢量 u RR. MERMER P 通过 绕 螺 旋 轴 旋转 ， 从 其 最 初 位 置 移动 到 
其 第 2 个 位 置 P'。 再 通过 平行 于 螺旋 轴 娘 移动 hh， 这 时 移动 至 P”"。 点 卫 的 初始 位 置 拓 量 用 
P 表示 ， 最 终 位 置 撩 量 用 p "RM. 

在 相关 参考 文献 中 ， 螺 旋 用 具有 螺 距 的 一 条 直线 表示 。 为 了 表明 运动 ， 需 要 增加 扭转 
角 。 因 此 ， 螺 旋 运 动 是 运动 的 螺旋 路 径 ， 扭转 就 是 实际 运动 。 然 而 ， 在 本 书 中 ， 我 们 定义 一 
个 具有 4 变量 函数 s Ch, p, ú, s) 的 螺旋 运动 以 便 说 明 这 个 运动 。 螺 旋 表 示 在 Ss ， 扭 转 
fa p 和 平移 h 时 螺旋 轴 运动 的 一 条 直线 。 

XF (Mozzi) 在 1763 年 首次 使 用 了 瞬时 螺旋 轴 ，1830 年 时 沙 勒 (Chasles) 发 现 了 螺 
旋 轴 。 

证 明 : 

角 轴 旋转 公式 [R 〈3-4)] 与 r 和 r 相关 ， CNES P 的 位 置 矢量 在 绕 着 旋转 f 
之 前 和 之 后 ， 当 s=0 MA=0 时 。 
r'=r cosg H(1— cosp) er Ut Xr )sing (4-152) 

然而 ， 当 螺旋 轴 不 通过 坐标 系 GC(OXYZ ) 的 原点 时 ，r 和 r 必须 相应 地 由 下 列 方程 所 
取代 : 



























































r=p—s (4-153) 
r'=p"—s —hú (4-154) 
这 里 的 r 是 旋转 之 后 在 G (OXYZ) 坐标 系 中 的 矢量 ; r 是 在 旋转 之 前 在 刚体 坐标 系 B 中 的 
矢量 。 
因此 ， 在 螺旋 运动 之 后 刚体 点 P 新旧 位 置 之 间 的 关系 如 下 : 
p"=(p 一 8 )cosg 十 (1 一 cosg)[。(P —s ) uta X (p —s ) Jsing +(s +hu) (4-155) 
对 于 最 常见 的 刚体 运动 , 式 4-155) 是 罗 德 里 格 斯 公式 。 定 义 新 标记 Sp =p" Alp = 
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p， 并 且 也 注释 s 表示 位 于 旋转 轴 上 的 一 点 ， 因 此 旋转 并 不 影响 s ， 我 们 可 以 提出 因子 ?p ， 
写 出 如 下 形式 的 罗 德 里 格 斯 公式 。 






































Gp =[Icosé tuuT(l—cosg) iusing ]® 一 LIcosg 十 zlTI(G1 一 cosg) 十 Using ]Ss 十 sg +hu 
(4-156) 
式 (4-156) 被 重新 调整 以 便 说 明 由 下 列 齐 次 变换 所 描述 的 螺旋 运动 。 
p=°Re p +° s —° Ros thu=o R pep +° d =°Tg? p (4-157) 
sc (SRB ee 了 
STp=°splh.d.uss = = (4-158) 
0 1 0 1 
式 (4-158) 中 
GRp=Icos$ +uu! (1—cos¢) +u sing (4-159) 
Sd=[((I—uu!)1—cosd) —wsing jss +hu (4-160) 
直接 替换 说 明 : 
uj vers¢ +c¢ uiu versd—Uu3Sh uiusversg Tuso 
SRp=| uiu2vers¢ tuss¢d u3vers¢ +c¢ u2u3versd—uis¢ (4-161) 
U1U3VerSg 一 XU2S uzu3versġ TU Sd usversg +c¢ 
hu; —uy (s3u3 Tsou2 151 U1 )versd + (s2uU3 —S53U2)Sh 151 Versd 
Sd=|huz—u2(s3u3+seu2tsiui)vers¢ + (s3ui —siu3)s¢+s2vers¢ 























hus —uU3(s3u3 Tsou2 siui )versd + (51 U2 —S52uU1) Sh +53 versd 
(4-162) 
刚体 运动 的 这 种 表述 要 求 有 6 个 独立 参数 ， 即 1 个 旋转 角 6. 1 个 平 动 位 移 h、 两 个 螺 
旋 轴 忆 、 两 个 位 置 矢 量 "s 。 这 是 因为 依据 式 (4-163),，z 的 3 个 分 量 彼 此 相关 。 
Ulu= (4-163) 
位 置 矢量 Ss 能 够 定位 螺旋 轴 上 的 任意 点 。 可 以 方便 地 选择 点 ， 在 该 点 处 距 原 点 o 具有 
最 小 距离 以 使 得 "s 和 慌 直 于 ww。 让 我 们 用 %s o 说 明 最 短 的 位 置 矢量 ,， 则 在 位 置 矢量 的 分 量 中 有 
一 个 约束 : 






































GsTú =0 (4-164) 
如 果 s 二 0， 则 螺旋 轴 通 过 坐标 系 G 的 原点 ， 式 (4-158) 可 简化 为 式 (4-145)。 
螺旋 参数 6 Ah PWR EHH co 和 位 置 矢量 56s 来 共同 定义 全 局 坐标 系 G (OXYZ) 中 的 刚 
坐标 系 B (ozyz) 刚性 运动 。 这 意味 着 ， 只 要 给 定 螺 旋 参 数 和 螺旋 轴 ， 就 可 以 通过 式 
(4-161) 和 式 (4-162) 找到 变换 矩阵 中 的 元 素 。 男 一 方面 ， 只 要 给 定 变 换 和 矩阵 5ST 8， 就 可 
以 通过 式 (4-165) 求 得 螺旋 角 和 螺旋 轴 。 














1 : 1 ; 1 
cosg => [Ltr Ra) 1]= 3Ltr Ts) —2]= 9 (ru | r 22 | r33 1) (4-165) 








1 
oS G —GRT = 
ü TTA Ra 一 GRI) (4-166) 
因此 ， 有 
r32 r23 
Pe 1 
4 sing Fis 131 (4-167) 
r21 r12 
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为 了 求 得 所 要 求 的 所 有 螺旋 参数 ， 我 们 也 必须 求 得 螺旋 轴 上 一 点 的 坐标 和 户 。 因 为 螺旋 
轴 上 的 点 在 旋转 之 下 是 不 变 的 ， 所 以 必 有 





rıl riz? 113 ria \(X 1 0 O hu,\(X 
ra rz rz ra|| Y 0 1 0 hu:||Y 
= (4-168) 
ral r32 r33 ra || Z 0 0 1 hus || Z 
0 0 0 1 il 0 0 0 1 1 
这 里 的 X, Y., Z) 是 螺旋 轴 上 点 的 坐标 。 





作为 一 个 样 例 点 ， 通 过 设置 X ,一 0 并 寻求 s = CO Y, Z,)7， 我 们 可 以 求 得 螺旋 线 与 
YZ 平面 的 交叉 点 。 因 此 ， 有 

















rui rie ris rı hu )( 0 0 
r21 r22—1 r23 roa —huez | | Ys 0 
r3i T32 r33 一 ]  r34—hus || Z 7 0 eo 
0 0 0 1 1 0 
式 〈4-169) 可 产生 用 于 求解 Y,、Z, Mh 的 3 个 方程 
h ui =r: Sry "(ria 
Ys | 王 |zx 1 一 r?? —rz r 24 (4-170) 
Z, u3 ra l—rss r34 
现在 ， 通 过 式 (4-171) 即 可 求 得 最 短 的 位 置 矢量 $80。 
Sso=s 一 (8 + Wu (4-171) 
WBN 4-30 ”基体 单位 矢量 的 中 心 螺旋 变换 。 








考虑 两 个 初始 重合 的 坐标 系 G (OXYZ) MB (oxyz)。 刚 体 沿 着 Y 轴 进 行 螺旋 运动 ， 
h=2 单位 和 $$ 二 90"。 可 通过 应 用 中 心 螺旋 变换 求 得 位 于 (1 0 0 DT 处 一 刚体 点 的 


























位 置 。 
hag td = 5 站 =DCPDR(7 于 
1 0 0 0/0 010 0 1 0 
lo A: OO. 100 10 2 memes 
“Te o ı Olt o o of Jar 00 0 
o 0o 0o ae 0 0 1 0 0 0 A 
因此 ， 有 
0 0 1 ŅA 0 
ci=s(2, =, j) i= a ee eh (4-173) 
2 —1 0 0 offo 一 1 
0 0 0 1A 人 1 1 
这 个 螺旋 运动 的 节 距 为 
1 5 1. 2732 单位 /rad (4-174) 


KB 4-31 点 的 螺旋 变换 。 
考虑 两 个 初始 重合 的 坐标 系 G (OXYZ) MB (ozyz)。 刚 体 沿 着 X=2 并 且 平 行 于 YY 
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轴 进 行 螺旋 运动 ，h 二 2 和 $$ 二 90"。 因 此 ， 刚 体 坐 标 系 位 于 (2 0 0)T 人 处。 刚体 点 在 3r = 
(3 0 0 1)7 处 的 位 置 矢量 可 能 通过 应 用 螺旋 变换 求 得 ， 即 

















0 0 1 2 
、 (y _ 0 1 0 2 
eT 一 = (4-175) 
0 1 =l 0 0 2 
0 0 0 1 
因为 
0 © l 
CRp=| 0 1 0 (4-176) 
一 ] 0 0 
2 0 
s 一 |0 | (4-177) 
0 0 
因此 ,Sr 的 位 置 矢量 为 
0 0 1 2/3 2 
TE E E OR Ole EA 
=1 0 | al 
0 0 0 Il 1 


KB 4-32 ”立方体 的 螺旋 变换 。 
考虑 如 图 4-17 所 示 的 立方 刚体 ， 该 刚体 棱 长 为 单位 长 度 ， 并 且 位 于 其 四 分 之 一 顶点 处 。 
WR SEA a 轴 将 立方 体 旋转 45"， 接 着 利用 sd Shu 平移 该 立方 体 。 











图 4-17 在 坐标 系 四 分 之 一 顶点 处 的 立方 刚体 


u= 1 DT (4-179) 
hu=(1 1 DT (4-180) 
这 时 相关 中 心 螺 旋 运 动 矩 阵 为 
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这 里 的 “Rs 就 是 罗 德 里 格 斯 变换 矩阵 。 


P : SRE hu GR, Cd 
STEp=°sp(h,$ ,0)= = (4-181) 
0 1 0 1 




















0. 80474 —0. 31062 0.50588 
SRp=Icosd Húú" 1 —cosd) +using=| 0. 50588 0. 80474 —0.31062} (4-182) 
一 0. 31062 0. 50588 0. 80474 





0.57735 
T ~ u 
p= — ,u=—==| 0.57735 (4-183) 
4 V3 
0.57735 
则 有 下 面 的 中 心 螺旋 变换 : 
0. 80474 —0. 31062 0. 50588 1 
、 0. 50588 0. 80474 —031062 1 
STR=Ssplhsdsu)= (4-184) 
—0. 31062 0. 50588 0. 80474 1 
0 0 0 1 


1 
h=1.¢6=n/4,u=—Q1 1 1)T 
f 万 


图 4-18 所 示 为 在 中 心 螺旋 运动 (ss h, ps ú) 之 后 的 单位 立方 刚体 。 











x 


X 
图 4-18 PUPA REIS AS h, po a) 之 后 的 单位 立方 刚体 


现在 假设 绕 着 wu 旋转 立方 体 ， 接 着 通过 hw 一 w 平移 该 立方 体 ，w 位 于 : 
Ss=(1 0 0)T (4-185) 
螺旋 运动 与 新 的 平 动 矢量 "d 具有 相同 的 旋转 矩阵 "Ra。 











1. 1953 
Cd 一 cs 一 5RBSs hú =[ A> úúT) 1—cosg)—u sing [Ss +hu=|0.49412| (4-186) 
1. 3106 
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则 有 下 列 中 心 螺旋 变换 矩阵 : 


Ss—GRp&s+hu 


l i 有 Rg 
CTg=C sgh 7) ,WU) 一 
0 1 


上 表面 顶点 的 局 部 坐标 系 的 位 置 矢量 为 


0 1 

Bre=|0|.2 rr=|0 

ji 1 
螺旋 运动 之 后 ， 上 述 各 顶点 位 于 sr =os 
1.701 2.506 
Srr=|0.183].° rp =| 0. 689 

2.115 1. 804 





图 4-19 所 描述 的 是 在 螺旋 运动 Gsp Ch, 





中 心 螺旋 运动 (sp Ch 


图 4-19 





iE 





h l, $ /4, 
T u 
V3 


KB 4-33 矢量 的 旋转 。 
变换 方程 Sr 二 SRpSr 和 罗 德 里 格 
的 旋转 运动 。 然 而 ,依据 固定 在 刚体 中 的 两 点 




















- 





0. 80474 —0. 31062 0.50588 1. 1953 
0. 50588 0. 80474 —0.31062 0. 49412 
—0. 31062 0. 50588 0. 80474 1.3106 
0 0 0 1 
(4-187) 
1 0 
Pro=l|1).2ra=ll (4-188) 
1 1 
B th, 6, us s) Pr 的 位 置 矢 量 为 
2.195 1. 390 
,rc 一 |1.494|, 5 ry =| 0. 988 (4-189) 
Zeal 2.62 
po ú, s) 之 后 的 单位 立方 刚体 。 
H 


> ġo ú, s) 之 后 的 单位 立方 刚体 





a 1 D's= (1 0 ODT, 


斯 变换 矩阵 [ 式 〈3-4)] 描述 了 国定 在 刚体 中 任何 矢量 





， 可 以 方便 地 描述 矢量 ,以 驱动 螺旋 运动 方程 。 


用 在 矢量 ri 尾部 的 参考 点 Pi 和 在 矢量 r? 顶端 的 参考 点 P* ， 定 义 刚体 中 的 矢量 。 这 时 ， 


刚体 坐标 系 和 全 局 坐标 系 之 间 的 变换 方程 可 以 写 为 


G(r2—r1)= 











GRp?(r2—r1) (4-190) 


假设 参考 点 Pi 的 最 初 位 置 和 最 终 位 置 都 是 沿 着 旋转 轴 的 ， 这 时 式 〈4-190) 可 以 被 调 
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整 成 如 下 合适 的 形式 ， 以 便 计 算 点 Po 在 变换 和 矩阵 形式 中 的 新 位 置 。 





Gr, =SRp? (r 27T] ) 十 srl =CRp ro +r, 一 CRPB 


式 (4-191) 中 


GRE Gr, —GRpBr, 
”T's 一 


0 1 


WF A=0 的 情况 ， 式 (4-192) 与 式 (4-158) 一 致 。 


KG 4-34 确定 螺旋 的 特殊 情况 。 


对 于 螺旋 运动 ， 有 两 种 特殊 情况 。 第 1 种 情况 出 现 ， 当 ri 一 ?2 





Bry =CTpPr, 


(4-191) 


(4-192) 


=r; =] f; 这 时 ， 














一 0， 该 运动 就 变 成 了 一 个 纯粹 的 平行 于 的 平 动 A， 即 有 
j= “Fy -e ae | Gm 
因为 在 这 种 情况 中 没有 特定 的 螺旋 轴 ， 因 此 不 能 确定 螺旋 轴 上 的 任何 具体 点 。 
当 % 王 180 时 ， 则 有 第 2 种 情况 。 在 这 种 情况 下 ， 有 
1/2G@n t1) 
ú =| V1/2(r2 FD 
1/2(r33 +1) 


然而 , h M X, Y, Z) 可 由 式 (4-170) 计算 。 
KG 4-35 平面 内 的 转动 和 平 动 。 

















假设 依据 图 4-20， 被 取代 的 平面 从 位 置 1 到 达 位 置 2。 














Q, (X,Y) 








Pi(1, 1) Q1(3,1) 









































图 4-20 平面 内 的 运动 


Q2 的 新 坐标 为 





ro 一 RiCraoi 一 Pi ) Frp, 


cos58” 一 sin58” 0)\((3 1 
一 | sin58” cos58” 01 一 11 
0 0 0 0 
1. 06 4 5. 06 
一 |1. 696 | 十 |1.5 |=] 3. 196 
0 0.0 





(4-193) 


(4-194) 


(4-195) 
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或 者 ， 等 效 于 


cos58” 一 sin58” 0 4.318\/3 

er ae rp,—*Rirp,| _|sin58 cos58” 00. 122 |} 1 
LG 1 R o 1 0 llo 
0 0 o 1 JŲ 








k 4-36 平面 运动 极 。 
在 刚体 平面 运动 中 ， 从 位 置 1 至 位 置 2 中 总 是 有 一 个 点 在 平面 内 运动 ， 
位 置 。 因 此 ， 可 以 认为 刚体 绕 着 这 个 点 转动 ， 这 个 点 被 称 为 有 限 转动 极 。 使 


























(4-196) 


并 没有 改变 它 的 
用 变换 矩阵 定位 





平面 极 。 图 4-20 所 示 为 一 个 三 角形 的 平 动 运动 。 为 了 定位 运动 Po (Xo, Yo) 极 ， 我 们 需 











要 运动 变换 。 利 用 图 4-20 所 给 出 的 数据 ， 有 


ca 一 Sa 0 一 ca 十 sa 十 4 
m| e sa ca 0 —ca—sa+l.5 
0 1 0 0 1 0 
0 0 0 1 
在 此 变换 下 ， 运 动 极 可 被 确定 下 来 。 因 此 ， 有 
rp, =°T1 rp, 
Xo cosa —sina 0 —cosa+sina+4 Xo 
Yo | _ lsine cose 0 —cosa—sina +1. 5 || Yo 
0 | | 0 0 1 0 0 
1 0 0 0 1 1 
xtFa=58°, A 
Xo =—1. 5sina+1—4cosa = 2. 049 


Yo =4sina +1—1. 5cosa=3. 956 
KI 4-37 ”确定 螺旋 参数 。 


(4-197) 


(4-198) 


(4-199) 
(4-200) 


当知 道 刚体 中 的 3 个 非 共 线 点 的 最 初 位 置 和 最 终 位 置 时 ， 我 们 就 能 够 确定 螺旋 参数 。 假 
设 用 po。、qo 和 ro 标记 点 P、Q、R 在 螺旋 旋转 之 前 的 位 置 ，p1、q1、r1 表 示 螺 旋 旋 转 之 后 





的 位 置 。 


为 了 决定 螺旋 参数 6、w、h 和 s ， 应 该 解释 下 列 3 个 联 立 的 罗 德 里 格 斯 方程 : 


了 1 一 了 0 一 tan Sax +p )—2s )+hu 


qı Qo tan 2X Cg +q)—2s )+hu 





ri —ro=tan Sai X (rı +ro 2s ) +hu 


式 (4-201) 减 去 式 (4-203), zh (4-202) 减 去 式 (4-203) 可 得 





(pi po) (ri ro)=tan 区 2X[CPi 十 po) 一 (ri 一 ro)] 





(qı =q) (r: ro)=tan Zú XE 十 go) 一 (rı—ro)] 
现在 ， 式 (4-204) 两 边 乘 以 垂直 于 xz 的 因子 [Cg 一 go) 一 (ri 一 ro)] 
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(4-201) 


(4-202) 


(4-203) 


(4-204) 


(4-205) 








[ (qi1—qo)— riro) XL(pi po) (ri ro)]=tan $a XE qo) (ri 0 1X 


{a XL(p,+po)—Cri—ro) J} 
(4-206) 
则 有 








L(qi qo) — Gri ro) |X Lp, +p.) — Gri ro) J tan Eg: qo)— (ri—ro)]* 


Lp: 十 Po) 一 (Crl 一 ro ) lu 








(4-207) 
因此 ， 通 过 相同 的 正切 tan( 5) aust (4-208) 右边 的 范 数 可 求 得 旋转 角 。 
(g —@.) —Gri — ro JX EG, 十 p) 一 (ri 一 ro) 
ee ia gem ANSE a = (4-208) 
2 Lgi go) — Grr — ro) ] « [pi pod — Giro] 
为 了 求 得 8 , ú 与 式 (4-201) LE, Æ 
iX (pı =P =| tan Sax (pı +P —28 +h | 
(4-209) 





Stan SC ie Ga es Sa 


TER. s es) es EETU 的 分 量 ， 此 处 s 是 从 全 局 坐标 系 G (OXYZ) 到 螺旋 

轴 上 任意 点 的 一 个 矢量 。 这 个 垂直 分 量 说 明 ， 在 点 O 和 螺旋 轴 刀 之 间 有 最 短 距离 的 一 个 秋 
H o 假定 So 是 最 短 的 Ss 则 有 

so=s —(i ia | ee 

tan(¢/2) 



































h 





出 





2 [ú e (pi +p.) i +p: tP | (4-210) 


螺旋 运动 的 最 后 参数 是 节 距 h ， 该 参数 可 由 式 (4-201), se (4-202) 和 式 (4-203) 中 
的 任何 一 个 方程 都 可 求 得 。 
h=u* (pi1—po)=u* (q —qQo) =u * (ri—ro) (4-211) 
KG 4-38 螺旋 变换 的 另 一 种 推导 。 
假设 螺旋 轴 没 有 通过 G 坐标 系 原点 。 如 果 Ss 是 螺旋 轴 上 某 点 的 位 置 矢 量 ， 则 我 们 可 
以 推导 螺旋 运动 y A, p, ús s) 的 变换 矩阵 ， 将 螺旋 轴 移 动 至 原点 ， 进 行 中 心 螺旋 运动 ， 
最 后 将 螺旋 轴 移 回 至 它 的 最 初 位 置 。 
s(h,$u,s )=D(%s )s(h.g.u)D(—8s ) 
=D (Cs Dh Rhu) D(—Ss ) 


| 4 GRE hu | =] GRg Ss—GFRposthi 
0 T 0 1 JW 1 0 1 


例 4-39 友 绕 非 中 心 旋转 轴 的 转动 。 
刚体 绕 着 通过 矢量 ss 所 表示 点 的 一 根 轴 旋转 运动 ， 在 该 运动 中 $s X uA 是 绕 着 非 中 
心 旋转 轴 的 转动 。 可 以 通过 设置 h 二 0 从 螺旋 变换 中 获得 与 非 中 心 转动 相关 联 的 变换 矩阵 。 
因此 ， 非 中 心 转动 变换 矩阵 为 
cRB cg 一 RbSs 
“ry a | (4-213) 


1 





(4-212) 
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交 例 4-40 主要 的 中 心 螺旋 运动 。 
有 3 个 主要 的 中 心 螺旋 运动 ， 即 绕 着 X 轴 的 螺旋 运动 、 绕 着 Y 轴 的 螺旋 运动 和 绕 着 Z 
轴 的 螺旋 和 运动， 其 中 心 螺旋 变换 矩阵 分 别 是 








cosa —sina 0 0 
sine cosa 0 0 
S(hz,a,K)= (4-214) 
0 0 1 p.a 
0 0 0 1 
cos8 0 sing 0 
Z P 0 1 0 p 
Syb. J= < (4-215) 
—sing 0 cosßg 0 
0 0 0 1 
1 0 0 PxY 
y 5 0 s —si 
SCS) Te a (4-216) 
0 siny cosy 0 


0 0 0 1 
克 例 4-41 WH CChasles) 定理 的 证 明 。 
假设 了 是 一 个 任意 空间 位 移 ， 将 其 分 解 为 一 个 绕 着 忆 的 旋转 运动 R 和 一 个 平 动 也 。 

了 一 DR (4-217) 
我 们 可 以 将 平 动 D 分 解 成 两 个 分 量 DI 、D | ， 分 别 表示 平行 和 垂直 于 旋转 轴 a 的 


分 量 。 























m 
































T=D\D(R (4-218) 
HE, DIR 是 一 个 平面 运动 ， 因 此 它 等 效 于 绕 着 平行 于 旋转 轴 i 的 转动 R' 二 DLR， 
这 服从 T=D | R' 这 样 的 分 解 。 这 种 分 解 完成 了 证 明 ， 因 为 D1 ASP a 

KG 4-42 ”每 个 刚体 运动 都 一 个 螺旋 运动 。 

为 了 说 明 任 何 正 确 的 刚体 运动 都 可 以 作为 一 个 螺旋 运动 考虑 ， 必 须 给 出 齐 次 变换 矩阵 : 


G Gq 
or 5 | (4-219) 





sk (4-219) 可 被 写 为 


、 GRp QA—ERpg)s+hú 
GTs= (4-220) 
0 1 
这 个 问题 相当 于 下 面 的 方程 ， 以 便 求 得 hh Ale 
Gd 一 (I 一 CRB)s +ha (4-221) 





矩阵 (1 一 GRs] 是 奇 的 ， 因 为 6SRs 总 是 有 一 个 为 1 的 特征 值 。 该 特征 值 对 应 于 将 如 作为 
特征 矢量 。 因 此 ， 有 








[I—CRg]ú =[I— R} Jú =0 (4-222) 





点 积 〈 内 积 ) 说 明 : 
ue Gd 一 六 [LI 一 CRB]s tus hv=[I-CRglu s +ù ° hu (4-223) 
由 式 (4-223) 可 得 
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h=ù » Sd (4-224) 
现在 ， 我 们 可 以 求 得 s 为 
s =[I—°R,]-1 (6 d 一 72) (4-225) 
KG 4-43 ”刚体 运动 的 分 类 。 
假设 一 刚体 坐标 系 BCozyz) 相对 于 全 局 坐标 系 G(OXYZ) 和 运动。 点 也 的 位 置 矢 量 是 

















ps 二 (Pi pz: p)”, 点 了 是 刚体 坐标 系 B (ozyz) 中 的 任意 一 点 。 刚 体 坐 标 系 B (ozyz) 
可 能 的 运动 和 坐标 系 之 间 所 要 求 的 变换 ， 可 分 成 如 下 类 别 : 
1) 绕 着 通过 原点 的 一 根 轴 万 转动， 该 轴 的 单位 矢量 如 二 (ul uz u)” 
cp 一 “RbB p (4-226) 











这 里 的 SRa 由 罗 德 里 格 斯 旋转 公式 [ 式 〈3-4)] 所 得 。 
2) Pood = (di dz d3)" 加 上 转动 Re 。 
Gp 一 GRBBP 十 cd (4-227) 
3) BARR a= (ur uz uz)? 轴 的 旋转 运动 4， 并 且 单 位 矢量 丸 通 过 由 5s = 
(s1 s2 s3)T 所 表示 的 任意 点 。 
Gp 一 GRBBP +65 一 GRBGS (4-228) 
4) Ses A A eH = (Cu ung) WIRED., We 上 MM h, J HIR E 
AN oc EHS s =(s1 s2 s3)" 所 表示 的 任意 点 。 
Sp 一 CRBB p +°s —ERpg°s +hu (4-229) 
D 偏转 运动 ， 分 以 下 5 种 情况 
a. 在 zy 平面 内 。 









































Sp 一 “RaBBp，。， (4-230) 
这 里 
Pi 
Po: 7| P2 (4-231) 
ps 
b. 在 yz 平面 内 。 
p=°Rep, ,,) (4-232) 
这 里 
pi 
Pe —| Pe (4-233) 
P3 
c. 在 zz FEN. 
p=" Ra" P45 (4-234) 
这 里 
Pi 
Poy =| P2 (4-235) 
P3 
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d. 在 平面 方程 为 uizx 十 usy 十 us3z 十 hh 二 0 的 平面 内 。 
; 1 : x ; é 
Sp “ou Te P —2hu) ="Rp® p —2hu (4-236) 
这 里 
—u? +u? +u? —2uiu2 —2u3uy 
GRg= —2u2ul u? 一 uz 十 2 — 2u2u3 (4-237) 
—2ui us —2uzu?2 ui 十 U2 一 3 
e 在 通过 点 (51 s2 ss ， 且 正 交 于 如 二 [ui wus us)" 的 平面 内 。 
Sp 一 cRBB p 十 58 一 5RBSS (4-238) 
ACR, 可 通过 式 (4-237) 求 得 
4.6 逆 螺 旋 运 动 
RJE MERE sh, gs ús s) 的 逆 矩 阵 定义 如 下 
| _ CRE SG8 一 GRSGS 一 AZ 
Gs 二 Byo(h o) Us ) 一 (4-239) 
0 1 


。 因 此 ， 中 





这 里 的 之 是 用 来 表示 螺旋 轴 的 一 个 单位 矢量 ; s 是 螺旋 轴 上 一 点 的 位 置 矢量 ; p 是 螺旋 角 ; 





h 是 螺旋 运动 的 轴 向 位 移 。 
如 果 螺 旋 运 动 是 绕 中 心 的 运动 ,那么 螺旋 轴 通 过 了 坐标 原点 ， 并 且 有 一 0 





心 螺 旋 运 变换 德 阵 的 逆 和 矩阵 为 
N f CRE —hu 
GsB!(h,$ d= (4-240) 
0 1 
WEAR: 
WAREZ Sth, g, ú, s) 的 齐 次 变换 矩阵 表达 式 为 
pe : GRB Ss—SRposthu 
STp=°splh.d.uss ) 一 (4-241) 
依据 式 〈4-23) ， 齐 次 矩阵 (4-242) 可 以 被 反 转 。 
GR 3 Gd 
“ry > | (4-242) 
0 1 
SRT —GpTG 
rors =( 0 (4-243) 
0 1 


为 了 说 明 式 (4-239) 的 正确 性 ， 我 们 需要 计算 一 cRESd : 
一 cREBSd 一 一 RE(CSsS 一 CRacs tha) 
一 一 GREGS 十 GREGRBGS —SRI hú (4-244) 





一 一 CREGS +s 一 人 REAL 
HF a 在 两 个 坐标 系 B 和 G 中 是 一 个 常量 矢量 ， 因 此 有 
i (4-245) 


û= Rg =R ů 





H 
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一 RESGd =s 一 REGS —CR Gh (4-246) 
KA T — PAR Tne es 5) A DARE HR PE 

















加 、 ERI Gs—SREGs—hu 
CSB (hod ,is )= | (4-247) 
0 1 
如 果 螺 旋 运 动 是 绕 中 心 轴 和 运动 的 ， 定 位 矢量 是 零 矢 量 并 且 螺 旋 运 动 的 逆 矩 阵 可 简化 为 
T i ee a 
"sp (h,$,u)= (4-248) 
0 1 
由 于 旋转 矩阵 "Ra 一 Ri 的 道 和 矩阵 可 由 一 个 绕 着 x 轴 旋 转 $ 角 的 转动 求 得 ， 即 
CR 一 GRE 一 BRG 一 Roy (4-249) 
Dl e WR Ie Js BY aE He SE u 轴 旋 转 一 $ PANE ONIN EA oc 轴 的 平 动 一 h 来 解释 。 
Osa (hed suss J=s he dats) (4-250) 
KG 4-44 验证 逆 螺 旋 运 动 公式 。 
利用 逆 螺 旋 公 式 ， 则 有 
SChopoties DSS hs, gs HL (4-251) 
验证 如 下 : 
GRB Cs—CRpg°s+hú\ (CR Ss—SRESs—hu 
| 0 1 0 1 | 
(Is CRe(Ss—GRESS—hu) + (Ss —GFRpos thi) 
= 1 
I; SRaSs 一 cg 一 /SRBZT 二 SS 一 cRBSS 十 Po Is 0 
-(' 1 -f > (4-252) 


KB 4-45 中心 螺旋 运动 的 道 变 换 。 
如 图 4-17 所 示 的 单位 立方 体 ， 绕 着 螺旋 轴 旋转 8 二 45"， 并 且 平 移 Sd Shi. 

u= 1 DT hai=( 1 1)7 (4-253) 
与 此 相关 的 中 心 螺旋 运动 矩阵 为 

















0.80474 —0.31062 0.50588 1 
- . Ris hu GR, Gd 0. 50588 0.80474 —0.31062 1 
5TB 王 SB( ,0)= = = 
0 1 0 1 一 0. 31062 0.50588 0. 80474 1 
0 0 0 1 
(4-254) 
TAM Iie 1 3 HY DE AE ME 
0.80474 0.50588 —0.31062 一 1 
oat . GRE hu —0. 31062 0.80474 0.50588 —1 
ree (hog su)= = (4-255) 
0 1 0.50588 —0.31062 0.80474 —1 
0 0 0 1 
可 以 由 和 矩阵 乘法 检验 逆 螺 旋 运 动 。 
GSB(h $sBlh, gu) = "sp (hog us sgh, p ú) = (4-256) 
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KG 4-46 一般 螺 旋 运 动 的 道 变换 。 
图 4-19 所 示 为 图 4-17 绕 着 螺旋 轴 Sw 转动 45" 并 且 平 移 hd =u (u 位 于 6s 处 ) 之 后 的 单 
位 立方 体 。 














1 1 1 
Gu =|1| húû=|1| 6s=I0 (4-257) 
1 1 0 
该 运动 的 螺旋 矩阵 为 
0. 80474 一 0. 31062 0. 50588 1.1953 
E i 0. 50588 0. 80474 —0.31062 0.49412 
GTg=€spg(h soUs )= (4-258) 
一 0. 31062 0. 50588 0.80474 1.3106 
0 0 0 1 
这 个 螺旋 运动 的 逆 和 矩阵 为 
0. 80474 0. 50588 —0. 31062 —0. 80474 
ea . GRE Ss—SRESs—hil 一 0. 31062 0. 80474 0. 50588 —0. 68938 
Gsp (h,$,u,s = = 
0 1 0. 50588 —0. 31062 0. 80474 —1. 5059 
0 0 0 
(4-259) 














通过 绕 着 螺旋 轴 Su 旋转 $ 一 一 45" 并 且 平 移 hw 一 一 w，w 位 于 6s 处 ， 可 以 将 立方 体 旋转 
回 它 的 原点 位 置 。 这 样 的 一 个 螺旋 运动 的 变换 和 矩阵 为 
0.80474 0.50588 一 0.31062 一 0. 80474 
一 0.31062 0.80474 0.50588 —0. 68938 








GT 一 (4-260) 
0.50588 —0.31062 0.80474 一 1. 5059 
0 0 0 1 
因为 ， 
0.80474 0.50588 —0. 31062 
GRy=Icos$ tau? (1—cos¢) +i sing =| —0. 31062 0.80474 0.50588 | (4-261) 
0.50588 —0.31062 0.80474 
1 1) /—1Y /~—0.80474 
Gs —GRBSs +hu] 0 |—SRp|0}+| —-1|=| 一 0.68938 (4-262) 
0 o} \-1 —1. 5059 





螺旋 运动 变换 矩阵 式 (4-260) 就 是 式 (2-58) 的 逆 和 矩阵 ， 因 此 它们 的 乘积 等 于 单位 矩 
BE L. 





六 4.7 复合 螺旋 变换 


假设 is， Chis gir ti, 81) 是 从 坐标 系 B 移动 到 坐标 系 Bi 的 螺旋 运动 ,ss hos 
%o，zo，8o) 是 从 坐标 系 Bi 移动 到 坐标 系 G 的 螺旋 运动 ， 则 从 坐标 系 Bo 到 坐标 系 G 的 螺 
旋 运 动 变换 为 
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Cs (h,pú,ss =es1 (ho ogo otto 80)! S52 Chisi sti 81) 
7 CRo GR, UI—!R2)s, +UI—-F R11) 80 HA FR iu +hoto (4-263) 
-| 0 1 ) 

证 明 : 

直接 取代 is2 (his $1, úis 81) PSs (ho, pos wos So): 














、 、 GR So —~EFRiSothou 
Ssi ro sgordosso)=] i ji ° | (4-264) 
ý 2 IR = s,—!Rosi thin 
1S2 (hisdisti-81)= 0 l (4-265) 
WA 
E = GR, so—PRisothotio\(CR2 sı —!Rosi thingy 
0 1 0 1 
o GR, GRi (Ss, IRs} hiú) | So GRiso houo (4-266) 
0 1 
(SR SR, CI—!R2)si t+ UPR )so thi Riu tho to 
0 1 
这 里 
SR: =®°R;ı'R? (4-267) 


WY RG E RJE Ss. h, 6. ud, s) 的 螺旋 参数 ， 从 基于 式 (4-167) 和 式 
(4-165) 的 SR， 中 求 得 和风。 利用 式 (4-225) 和 式 (4-225) AURIE h Alls: 

h=ù » d (4-268) 

5 一 [LI 一 SR] (Cd —hi) (4-269) 





w 
HE 


Sd =R; (1—!R2)s, +A—ERı)so +hi fF Rit thouo 
=(SRı —ER2)sı +R) (hii —80) +80 thouo 
KB 4-47 螺旋 运动 的 指数 表述 。 
为 了 计算 与 螺旋 相关 联 的 刚体 运动 ， 考 虑 图 4-16 中 点 忆 的 运动 。 点 的 最 终 位 置 可 由 式 
(4-271) 确定 : 


(4-270) 














p=s ter +hi=s +e* (p—s )+hu=T p (4-271) 
这 里 的 工 就 是 螺旋 运动 的 指数 表述 。 
( TE 
T= 
0 1 
式 (4-272) 中 的 指数 螺旋 变换 矩阵 是 以 罗 德 里 格 斯 公式 [ 式 (3-123) ] 的 指数 形式 为 基 
础 的 。 


(4-272) 


























ew =I+u sing Hú? (1—cos¢) (4-273) 
因此 ， 两 个 螺旋 运动 的 合成 运动 也 可 由 式 (4-274) 求 得 : 
aie aie ae EE 


efit 
T=TiT;= 
0 1 0 1 


jeu: (4-274) 
e 1Uu 1 2u2 x 
-| 0 °] 


137 


Ee wens, 


学 : 运动 学 、 动 力学 与 控制 技术 





这 里 


Gd 一 (eg —esiti t4202 )82 tes" (hotig—si) tsi thie 
KB 4-48 ”两 基本 中 心 螺旋 运动 的 合成 。 
两 个 基本 中 心 螺旋 运 动 的 合成 可 由 矩阵 乘法 求 得 。 作 为 一 个 例子 ， 先 绕 着 Y 轴 进 行 螺 




















(4-275) 


旋 运 动 ， 接 着 绕 X 轴 进 行 螺旋 运动 ， 其 变换 矩阵 为 
Lt 0 0 YPx cB 0 sp 0 
Yee nee 8 E 0: Gy sy 0 0 1 0 Bpy 
0 sy cy 0 —sB 0 cB 0 
0 0 0 1 0 0 0 1 
cospB 0 sinf YPx eres 
sinBsiny cosy —cos@siny Bpy cosy 
7 —cosysinß siny  cosBcosy Pp ysiny 
0 0 0 1 
螺旋 合成 不 可 以 互 换 ， 因 此 
SCOPx yyCyr Bot AS hyb JS Chx sot) (4-277) 
KI 4-49 螺旋 运动 的 分 解 。 
每 个 常规 螺旋 运动 都 可 以 被 分 解 成 3 个 基本 的 中 心 螺旋 运动 。 
> 、 (5 Rte 
Gsp(h,$ ,Us ) 一 
0 (4-278) 
=s (hx, Y Ds (hy 8 I) shz,a,K) 
因此 
ca cp — cBsa sp 
GRp=| cy+casßsy cacy—sasBsy —cBsy (4-279) 
sasy—cacysß casy 十 cysas8 cBey 
Yp x Tap zsing dx 
s —ERg s +thu=|Bpycosy—apzcosfsiny |=|dy (4-280) 
by siny +ap z cosßcosy dz 
由 旋转 变换 矩阵 “Ra 可 求 得 扭转 角 a、B、Y。 这 时 求 得 的 节 距 px、py、pz WTF: 
dzcosy—dysiny 
pz= anas (4-281) 
d zsiny +dycosy 
Py B (4-282) 
ps -2 sing (4-283) 
KB 4-50 基本 中 心 螺旋 运动 的 分 解 。 


一 般 说 来 ，3 个 基本 中 心 螺旋 运动 有 6 种 不 同 的 独立 组 合 ， 因 此 有 6 种 不 同 的 方法 将 一 
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般 螺旋 运动 分 解 成 基本 中 心 螺 旋 运 动 的 组 合 。 它 们 是 : 
shax, y, Tlhy, B, Ehz, a, R 
s(ay, B, J)s(hx, y, Ts(hz, a, K 
s(hz» a, KJs(hx, y, f)s(hy. B. T 
shz, a, KJs(hy, B, Elax, y. Tf 
s(hy, B, J)s(hx, y, (hz a, R 
s(hx, y, T)s(hz, a, K)s(hy, B. fF 
基本 中 心 螺旋 运动 的 6 种 合成 的 扩展 形式 见 附录 C。 它 说 明了 每 种 螺旋 运动 被 分 解 成 3 
种 基本 中 心 螺旋 运动 。 


Js 
}s 


(4-284) 





4.8 EVE (Pliicker) 坐标 





对 于 显示 一 条 线 的 最 常见 的 坐标 就 是 普 昌 克 (Plucker) 坐标 。 如 果 我 们 有 一 条 线 
的 两 个 不 同 点 的 位 置 ， 那么 就 可 以 求 得 该 点 的 空间 解析 描述 。 假 设 位 于 ri1 和 rs 处 的 两 
点 Pi (Xi, Yi, Z1) MP2 (X2, Yo, Z2) 是 位 于 如 图 4-21 所 示 直 线 1 上 的 两 个 不 
同 点 。 

利用 位 置 矢量 r1 Mrz, HR 1 的 方程 可 由 这 两 个 矢量 的 6 个 元 素来 定义 : 



































È 
M 
ún IN 
=(")= (4-285) 
P 
Q 
R 
这 6 个 元 素 作 为 直线 7 的 善 吕 克 坐标 。 这 里 
a= T1 -LÎ+M]+NÉ (4-286) 
lro—ril 








它 是 一 个 沿 着 直线 的 单位 矢量 〈 方 向 矢量 )， 
此 有 
p =riXu=PI+QI+RK (4-287) 

CERRAH a RERE. 
普 昌 克 方 法 是 一 种 公认 的 直线 定义 表述 方法 ， 
因此 它 比 其 他 方法 更 有 效 ， 例 如 参数 形式 1 (1) = 
ri 十 tw ， 方 向 形式 Gi, å), 或 者 来 自 Gn, ro) 
的 两 点 表述 形式 。 

UE AR: 

单位 矢量 ú 的 方向 是 沿 着 连接 Pi A Po 的 直 
线 ， 于 是 有 




















图 4-21 


两 点 表示 的 一 条 直线 
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ws: 运动 学 、 动 力学 与 控制 技术 





ro Fi X2 一 X1、 























i= ae FUTNR= cme a eer ame bc meee 
这 里 
L?+M?+N?=1 (4-289) 
两 点 P1 (Xi, Yi» Z1) MP (X2, Yo. Z2) 之 间 的 距离 为 
d= /(X2—X1)? +(V¥2—-Y1)* +(Z2—-Z))" (4-290) 
如 果 r 代表 着 从 原点 O 到 达 直 线 1 上 一 点 的 一 个 矢量 ,那么 矢量 r 一 ri 平行 于 矢量 ú, 
因此 直线 方程 可 以 被 写成 
(r —ri) Xu=0 (4-291) 
或 者 等 效 于 
rXi=p (4-292) 
这 里 
p=ri Xu 








它 是 绕 着 原点 O 的 方向 矢量 ww Ke. MA. AA KRitp 和 ww 相互 垂直 ， 所 以 在 它们 
的 分 量 之 间 有 一 个 约束 : 


























ú ep =0 (4-293) 
扩展 式 (4-287), WA 
i J K 
p=|iX! Yı Z,|=PI+QI+RK (4-294) 
L MN 
这 里 
P=YIN—Z1M 
Q=ZiL— XIN 
R=XiM-—YiL (4-295) 
因此 ， 式 (4-293) 的 正 交 条 件 可 被 表述 为 
LP+MQ+NR=0 (4-296) 
直线 (4-285) 的 普 吕 克 坐 标 有 4 个 独立 坐标 ， 因 为 有 两 个 约束 ， 分 别 为 式 〈 人 289) 和 
xk (4-296). 


在 式 (4-285) 中 ， 普 吕 克 坐标 的 排列 是 直线 排列 ， 它 被 称 为 射线 坐标 ， 然 而 有 时 
轴 排 列 L= Cp 如 7 中 的 反 转 顺序 也 在 其 他 书 中 使 用 。 在 男 一 种 情况 ， 季 线 
l OTRE Cis p)? 可 以 被 用 来 从 后 3 个 元 素 中 分 离 出 前 3 个 元 素 。 两 种 排列 























可 有 效用 于 运动 学 之 中 。 

MAW Bin Cp)? 是 齐 次 的 ， 因 为 式 (4-287) 表明 坐标 (wi wo)T，(wER), $ 
定 同一 直线 。 

力 - 转 矩 ， 角 速度 - 平 动 速 刚体 运动 像 直线 矢量 一 样 动作 ， 它 们 可 以 在 善 吕 克 


(Pliicker) 坐标 中 描述 。 
KG 4-51 连接 两 点 直线 的 普 吕 克 坐 标 。 
连接 点 P; (1, 0. 0) MP: O, 1, D 直线 的 普 吕 克 直 线 坐 标 为 
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l | 1 1 0 —1 DT (4-297) 
因为 
EA ee (4-298) 
|r2—ri| 
V3p=ri X/3a=—J+K (4-299) 


WK 4-52 ”立方体 对 角 线 的 普 吕 克 坐标 。 
图 4-22 所 示 为 一 个 单位 立方 体 和 相 邻 两 面 对 角 线 上 的 两 条 线 。 连 接 顶 点 P! (1，0，1) 
和 顶点 P，(0，1，1) 的 直线 X 






































图 4-22 单元 立方 体 























M T 
n=($:)-{ v2 V2) 2 _ v2 a (4-300) 
1 2 2 2 2 
因为 
| (4-301) 
[pz 一 Pi J2 
—Î—Î +K 
pi=piXti=— (4-302) 
连接 顶点 Qi A, 0. 0) 和 顶点 Qi C, 1, 1) WERL X 
T 
n=f Ho 2 2 0 V2 | (4-303) 
P: 2 2 2 2 
因为 
PE eee (ee Se (4-304) 
lqz—qil JE 
一 了 十 KK 
P: 一 gl1 义 人 ?一 (4-305) 











KB 4-53 FAG HLT SE RN AB he o 
可 以 验证 : 对 于 两 点 坐标 ， 如 果 定 义 如 下 行列 式 的 话 ， 








CD 


ED 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 























Wy X1 Yı Z1 
L= P= (4-306) 
we X2 Ya Zə 
Tl Yı Zi Xı 
M= Q= (4-307) 
ZU2 Yo Z2 X 2 
Wy Z\ XI Yı 
= R= (4-308) 
w2 Z2 X2 Y2 
格拉 斯 曼 和 矩阵 就 是 普 吕 克 坐 标的 简短 标记 。 
Wy X Yı Zı 
(4-309) 
w2 X> Y2 Z2 


KG 4-54 光线 轴 的 排列 变换 。 
可 以 验证 : 利用 式 (4-12) Ask 〈4-13)， 善 吕 克 坐标 的 线 排列 [ 式 (4-310)] 可 以 被 转 
换 至 轴 排 列 [ 式 〈4-311)]， 反 之 亦 然 。 

















u 
tw=| | (4-310) 
p 
ce (4-311) 
u 
国宝 同 (4-312) 
a) |p 
000100 
000010 
000001] fo & 
A= = (4-313) 
10000 0] Wy o 
010000 
001000 
ARMIE PEA FET PK FE ME. I LTR EP Uy FE 
2=AA=I (4-314) 
AT 一 人 (4-315) 





KB 4-55 普 昌 克 坐 标的 分 类 。 

如 图 4-23 所 示 ， 普 昌 克 坐标 有 以 下 3 种 情况 : 图 4-23a 所 示 的 一 般 情况 ; 图 4-23b 所 示 
的 线 通 过 原点 ; 图 4-23c 所 示 的 线 在 无 限 远 处 。 

一 般 情况 的 := (i o)T， 就 是 当 立 和 p 非 零 时 的 情况 。 方 向 矢量 立 平 行 于 直线 ，p 正 交 
于 含有 原点 和 直线 的 平面 ，| o | 表示 从 原点 到 直线 的 距离 。 

直线 通过 原点 的 情况 的 1 二 (x OT 就 是 当 直线 通过 原点 和 p =0 时 的 情况 。 

直线 在 无 限 远 处 情况 的 1 二 (0 M 就 是 直线 到 原点 的 距离 趋 于 无 穷 大 。 在 这 种 情况 
下 ， 我 们 可 以 认为 立 等于零。 当 直 线 位 于 无 穷 大 时 ， 最 好 重新 定义 普 昌 克 坐 标 ， 通 过 归 


We AR it SEM. 
=E i) (4-316) 
lol lol 


因此 ， 直 线 的 方向 分 量 随 着 距离 而 趋 于 零 ， 与 此 同时 和 矩 分 量 则 保持 有 限 性 。 
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a) b) c) 


图 4-23 A ARP AY 3 种 情况 
a) 一 般 情 况 pb) 线 通过 原点 oc) 线 在 无 限 远 处 











可 以 认为 位 于 无 穷 远 处 的 直线 是 垂直 于 和 矩 矢量 的 所 有 平面 集 的 交叉 点 ， 可 以 证 明和 矩 矢量 
可 以 完全 确定 这 条 直线 。 

ELAR (0 0 0 0 0 0)7 是 不 能 定义 直线 的 。 

KI 4-56” 线 矢量 变换 。 

考虑 如 图 4-24 中 所 示 的 直线 87， 它 是 在 刚体 坐标 系 B (Cozyz) 由 式 (4-317) 所 定义 的 
直线 。 











图 4-24 在 B 坐 标 系 和 G 坐标 系 中 的 一 个 线 矢量 








By By 
"| ]- i (4-317) 
Bp Brp X Bù 


这 里 , a 是 平行 于 直线 7 的 一 个 单位 矢量 ; 点 书 是 直线 上 的 任何 点 。 在 全 局 坐标 系 G 
(OXYZ) 中 直线 的 普 吕 克 坐 标 可 以 用 式 (4-318) KR: 


, Gù Gù 
G] ee je (4-318) 
P erp xu 


cù =° Rp (4-319) 


这 里 


Gy 绕 着 点 O 的 矩 Sp 为 
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D 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





Cp=S rp XG=( do +S RpPrp) X© Rp 
=6 do XS Rgëů +° Rp@ rp XBA) 
=G do Xs Rpg’ +E Rp? p=" do Rpg’ +E Rgëp 
对 于 线 矢 量 的 6X1 FB mE A PMT 可 以 从 坐标 系 B 转换 至 另 一 坐标 系 G。 


(4-320) 











SI=C Tpl (4-321) 
HER | (4-322) 
°p "Bo 
式 (4-322) 中 的 6X 6 变换 矩阵 定义 为 
、 SRB 0 
°Tp=|. 、 ‘ (4-323) 
Gdo Ry nl 
这 里 
ri riz ris 
GRs=|rz2 rz rz (4-324) 
r31 T32 T33 
0 as d2 
Sdo=| d; 0 —d (4-325) 
—d2 dı 0 
d2r31—d3r2i dor32—d3r22 dor33 —d3r28 
CdoSRy=|—diratdsrn —dirz tdri? —dirsstdsris (4-326) 
dira—dori dir22—dorie dir23—doris 


友 4.9 线 几 何 和 面 几 何 


对 于 两 条 直线 间 的 矢量 憩 、 最 短 距 离 和 夹 角 ， 普 吕 克 坐标 是 一 种 合适 的 方法 。 
*4.9.1 Kee 

考虑 如 网 4-25 所 示 的 任意 两 条 直线 li) = (i pi)? MSC po)’. FHA] 和 
直线 1，。 上 的 点 Pi 和 Ps 分 别 由 矢量 ri Mro 表示 。 直 线 的 方向 矢量 是 2 Made. 

直线 1 RAA Pi REEE Cro—r1) Xo， 


我 们 可 以 由 式 (4-327) 定义 直线 lo 绕 着 直线 /1 WK 
量 和 矩 。 












































lp XI, =a) *° (rə —rı)Xùů? (4-327) 





FA oti er Xo =e a Xr, WEI (4-327) 























可 简化 为 
1 X1, =ù; * pe tû: + pi (4-328) 
FH EL ot Ae to Fr SH aR AY AR AY HR BO 
积 可 定义 如 下 : 图 4-25 任意 两 条 斜 线 
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ûz Ul 
2xa=| el Ja e pe tû: > pi (4-329) 
pp? Pi 


互 换 积 具有 互 换 性 ， 并 且 可 以 给 出 两 条 方向 直线 的 矢量 矩 。 
妇 4.9.2 角度 和 距离 


WR d 是 两 条 直线 0=(Ca o) MS: p)" 之 间 的 最 短 距离 ，a EL0,xj 是 两 
条 直线 Li All. 的 夹 角 ， 则 有 











sina = |t1 Xù? | (4-330) 


ofl 


1 | [i 
~ sina P? 

因此 ， 当 且 仅 当 两 条 直线 的 互 换 积 为 零 时 ，/; 和 71s 这 两 条 直线 相交 。 可 以 认为 两 条 平 
行 线 在 无 限 远 处 相交 。 对 于 平行 直线 ， 距 离 表达 式 并 不 起 作用 。 


*4.9.3 平面 和 线 
具有 正 交 单位 矢量 刘 =n11 十 nj +n; K 的 平面 sz 的 平面 方程 是 





1 
1° 2° 二 一 一 x - 
| ay prota. pi | Sing | 1 Le | (4-331) 


sina 















































ny X +neY¥+n3Z=s (4-332) 
这 里 ，s 是 平面 距 原点 O 的 最 小 距离 。 我 们 可 以 利用 齐 次 表述 说 明 一 个 平面 。 
m=(n, no n3 s)T (4-333) 
对 于 位 于 平面 内 一 点 的 矢量 r=(X_ Y Z w), USHK aT. r=0, B 
X 
mT er=(ny nz n3 s) A =0 (4-334) 
Z 
w 
WA. w=0 说 明 所 有 点 位 于 无 穷 远 人 处，s 二 0 说 明 所 有 平面 都 含有 原点 。 
平面 x 与 X 轴 的 交点 ， 或 者 X HAN EB AE X= s/n, Y 轴 的 截 距 是 了 二 一 s/ns， 以 
































KZ 轴 的 截 距 是 Z 二 一 s/n3。 如 果 ns 二 0， 那 么 平面 垂直 于 XY 平面。 如果 n2=n3=0, HE 

么 平面 垂直 于 X 轴 。 对 于 其 他 平面 和 轴 也 有 类 似 的 条 件 。 如 果 在 式 (4-33) 所 述 的 平面 内 
有 一 点 (Xo, Yo, Zo), WA 

nı(X—Xo)tn:(Y¥—Yo)+n;(Z—Zo)=s (4-335) 

平面 内 一 点 (X Y Z w PRAWE: 


2 2172 
d= a = 2 (4-336) 


与 此 同时 平面 x 二 (m1 m m, s)T 距离 原点 的 距离 是 . 





















































dz 一 (4-337) 


2 2 2 
mi 十 m3 十 m3 


连接 位 于 矢量 ri 和 矢量 r; 人 处 的 两 个 点 Pi (X1，Y1，Z1) MP: (X22, Yo. Z) 的 直 
线 方程 可 表述 如 下 : 
l=ri+m(r2—-nr)) (4-338) 
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ws: 运动 学 、 动 力学 与 控制 技术 





AP (X, Y, Z) 距离 直线 1 上 任意 一 点 的 距离 由 式 (4-339) 确定 : 





d?=[ X; tm(X >» X 1) x |? | LY) tm (Ys Y1) 





对 于 式 (4-340) 而 言 ， 式 (4-339) 具有 最 小 值 。 


Al Bs 


m(Z2 


Z1) 


(X2—X1) CX 1 — XI) 4+ V2 -Y1) V1 Y) + 22-21) Z1-Z2) 








m 








Xe— Xi + YY HZZ 


为 了 求 得 距 原点 的 最 小 距离 ， 我 们 设 定 X=Y=Z=0. 


KB 4-57 立方 体 相 邻 两 条 对 角 线 的 夹 角 与 距离 。 








如 图 4-22 所 示 的 单位 立方 体 中 两 条 对 角 线 的 普 吕 克 坐 标 为 





V2 v2 V2 





TREDE 


2 2 2 





直线 /0; 和 2， 的 夹 角 为 





3 
a=arcsin | ûz X a1 | =arcsin z 560 
它们 的 距离 为 
il Ul U2 1 z 中 2 
= | el = |i. * pe tû * pıl=—=|—0.5|= 
sina | |p; Pp? sina (3 





例 4-58 一 点 距离 一 条 直线 的 距离 。 

连接 两 r 和 r2 端 点 的 直线 方程 分 别 是 : 
rm=(-1 2 1 DT 
72 一 (1 一 2 一 1 1)7 





TL 
2 z) 


ee ea 


—1+2m 
2—4m 
1—2m 


l=r, t+m(r2—-r1)= 


1 


1 
B 


ME, 矢量 r3 二 (1 1 0 DT 端点 和 直线 7 的 距离 由 式 (4-348) 确定 : 








s? =[Xi1+m( (Xs—X1)—X]+[EY +m(Y:—Y1) 
二 24m? —20m +6 
对 于 式 (4-349) Mia. sk (4-348) 具有 最 小 值 。 
2 
12 








m 











Y| +Z 


因此 ， 矢 量 rs 端点 的 最 小 距离 处 在 直线 上 的 点 的 矢量 为 

















二 人 
6 3 6 


例 4-59 ”两 条 直线 之 间 的 距离 。 
连接 两 矢量 ri 和 r2 的 端点 的 直线 : 
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工 


m(Z2 


Z1) 


Zz} 
(4-339) 


(4-340) 


(4-341) 


(4-342) 


(4-343) 


(4-344) 


(4-345) 
(4-346) 


(4-347) 


ZF 
(4-348) 


(4-349) 


(4-350) 














第 4 章 ”运动 的 运动 学 
7r1 一 (一 1 2 1 1)7 (4-351) 
7r2 一 (] 一 2 一 1 1)7T (4-352) 
—1+2m 
l=ritm(r2—-n1)= Bi (4-353) 
1—2m 
1 
ERREA rs 和 r4 的 端点 的 直线 : 
7r3 一 (] 1 0 DT (4-354) 
ri=(0 一 1 2 1)7 (4-355) 
=n 
b=rstncri—rsd=| (4-356) 
2n 
1 
直线 上 任意 两 点 之 间 的 距离 平方 为 
5 二 (一 1 十 2m 一 1 十 n 人 ?十 (2 一 4m 一 1 十 2n)? 十 (1 一 2m 一 2n)? (4-357) 
最 小 距离 可 由 相对 于 m 和 7 的 最 小 值 求 得 。 
m =Q. 433 ,7 一 0. 321 (4-358) 
这 样 一 来 ， 分 别 在 两 条 直线 上 具有 最 小 距离 的 两 点 是 : 
—0. 114 0. 679 
0. 228 0. 358 
ra | i (4-359) 
0. 114 0. 642 
1 1 





Kl 4-60 两 条 直线 相交 的 条 件 。 





如 果 两 条 直线 L= pOT MLS: po)? 相交 ， 并 且 它 们 共同 点 〈 即 交点 ) 的 





位 置 位 于 r 的 端点 处 ， 则 





pı5rXùůı 
PpP: 5r Xu?» 
因此 ， 有 
Pi t2=(r Xt) ° ûù25r (NX) 
po ù= Xü) >ur Sr (a2 Xú) 
1° p2 Fúûz * pi=0 
或 者 等 效 于 


KG 4-61 旋转 轴 的 普 昌 克 坐 标 。 
考虑 相对 于 在 XY 平面 内 平 动量 绕 着 Z 轴 旋 转 a 角 的 齐 次 变换 矩阵 为 





(4-360) 
(4-361) 


(4-362) 
(4-363) 


(4-364) 


(4-365) 
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rll rz 0 Xo 
、 ra rz 0 Yo 
STp= 
0 0 1 0 
0 0 0 1 
由 于 
cosa —sina 0 Xo 
、 sina cosa 0 Yo 
CT,p= 
0 0 1 0 
0 0 0 ji 
通过 比较 可 获得 旋转 角 为 
Ti 
a =arctan — 
rıl 


通过 搜索 在 两 个 坐标 系 中 具有 相同 坐标 的 一 个 点 ， 可 以 求 得 旋转 极 。 




















ri riz 0 Xo\/X, Xp 
ra r2 0 YollY,| |Y; 
0 0 1 olol lo 
0 0 0 1 八 1 1 
st (4-369) 可 以 被 写 为 
rii—l r12 0 Xo\/X, 0 
r21 r2—1 0 YollY,| JO 
0 o 1 0llo|l fo 
0 0 0 tli 1 
这 些 方程 的 解 为 
rebate web hay 
Ss er ar a re 





AHH AES A AR l= op) 等 于 : 
1 二 (0 0 1 Yp —X>» 0)T 


4.10 ”螺旋 坐标 和 普 吕 死 坐 标 








(4-366) 


(4-367) 


(4-368) 


(4-369) 


(4-370) 


(4-371) 


(4-372) 


(4-373) 


考虑 由 普 昌 克 坐 标 (% PMT 所 表述 的 螺旋 运动 y h, g, ù, s), EM WERE p= 


/8 。 螺 旋 可 由 普 昌 到 坐标 定义 。 


apal Fl tes) lepraa) 
Sy le) lptpi) l$ p+hů 











(4-374) 


如 果 节 上 距 是 无 穷 大 ， 即 p 二 oO， 那么 螺旋 运动 则 退化 为 纯 平 动 ， 或 者 等 效 于 在 无 穷 远 处 


的 一 条 直线 。 
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0 
5h 0.7) =| i) (4-375) 
hu 





Fes HE AHR WE IS BH pp = 0 相当 于 一 个 纯 转 动 ， 这 时 螺旋 坐标 等 同 于 螺旋 线 的 普 昌 


坐标 。 
pu ù 
SC0， 几 ,7 ,8 ) 一 =] (4-376) 


中 心 螺旋 运动 由 通过 原点 的 一 条 直线 定义 。 


Sh $ ù)=s(h,ġ.ù,0) [* ie 

pu hu 

在 机 器 人 速度 分 析 中 ， 微 分 螺旋 运动 的 螺旋 坐标 是 很 有 用 的 。 考 虑 一 螺旋 轴 SB 

旋 轴 / 的 角速度 w 二 wi 二 $i ， 沿 着 螺旋 轴 1 的 速度 w。 如 果 局 部 矢量 s 位 于 螺旋 轴 /! 上 一 点 
的 位 置 矢量 ， 则 螺旋 轴 HIE A be Ae on 











| Dh DR, g) (4-377) 














u ù 
:-| 上 | | (4-378) 

p sxXu 
螺旋 节 距 为 

p= (4-379) 
螺旋 方向 可 由 式 (4-380) 定义 。 

(4-380) 

| @ | 


因此 ， 瞬时 螺旋 坐标 多 (o, Ws Us s) 是 : 


T 1 四 
brosi = (wi etre) | H | | (4-381) 
|@ | s Xut+v s Xutpo 





KB 4-62 ”有 瞬时 螺旋 运动 的 节 距 。 
由 普 吕 克 坐 标 所 定义 的 瞬时 螺旋 运动 的 节 距 ， 可 由 式 (4-382) 求 得 。 








p= -£g (4-382) 
因为 两 个 普 吕 克 矢 量 是 正 交 的 , wp 二 0， 因 此 有 
gi peu (ptd purd=(du-r-pt¢?p)=s'p (4-383) 


wl 4-63 ”螺旋 轴 上 上 距离 原点 最 近 的 点 。 
位 于 螺旋 轴 上 且 距 原点 最 近 的 点 可 由 下 列 的 位 置 矢量 求 得 。 
So=guX¢e (4-384) 




















4.11 本 章 小 结 





相对 另 一 坐标 系 G， 刚 体 B 的 任意 运动 被 称 为 刚体 运动 ， 可 由 式 (4-385) 表述 。 


crp 一 SRBBrp 十 cd (4-385) 
这 里 

Srp=(Xp Yp Zp)" (4-386) 

Brp 一 (zP yp zp)T (4-387) 
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Gd=(Xo Yo Zo0)T (4-388) 
相对 于 坐标 系 G， 当 564d = 二 0 时 ， 矢 量 64d 是 平 动 ,SRs 是 将 矢量 Br 变换 至 cr 的 旋转 变换 
矩阵。 
引入 一 个 点 的 齐 次 坐标 : 
WX X 
r=| |=|” (4-389) 
WZ zZ 
w 1 
我 们 可 以 将 旋转 运动 和 平 动 合成 在 一 个 4X4 齐 次 变换 矩阵 之 中 ， 式 (4-390) 说 明了 坐标 
变换 。 
cr 一 “TB r (4-390) 
这 里 
& 了 
STp= (4-391) 
0 1 








FH FEU AB REIS T IFA ZEIE ZCI. A A REST Bg! 当 符 合式 4-93) 时 才 有 式 
(4-392) 成 立 。 


E 加 GRE —GRIGCgd 
iT, = fy, = (4-392) 
0 1 
GTs!'° Tp=L (4-393) 
刚体 运动 也 可 以 用 螺旋 运动 h, bs ù, s) 和 螺旋 变换 表达 

Sr=Ssth.g.ua.s)?r (4-394) 

、 、 GRB Ss—SRpSosth i 
Sth ,gu ,SI) 一 0 1 (4-395) 


螺旋 运动 h, p, ù, s) 可 由 以 下 螺旋 参数 说 明 : fe eee d EAA, Jani 
矢量 s 、 旋 转角 p 和 平 动 位 移 h RAWE p)。 局 部 矢量 s 表 明 螺 旋 轴 上 一 点 的 全 局 位 置 。 
当 s=0 时 ， 旋 转轴 站 通过 坐标 系 的 原点 ， 螺 旋 运 动 被 称 为 中 心 螺旋 运动 了 (Ch, ps å), 
个 螺旋 运动 都 可 以 被 分 解 为 绕 着 全 局 坐标 系 G 中 的 3 个 轴 旋 转 的 3 个 基本 的 中 心 螺 旋 运 动 。 









































一 个 刚体 运动 可 以 更 有 效 地 由 螺旋 运动 和 普 昌 克 直 线 坐标 表示 : 
> ù ù pu 
sonst EL EL, fe ca 
p=rXi (4-397) 


习题 
4-1 标记 与 符号 。 
描述 下 列 这 些 符 号 的 含义 。 
Dd 2)°Ts! 3)8TG 4)cGDB 5)Sšg Che $, ù) 


6) Dan 7) 57! 8)s 一 Fa 9) | 10)Sšg (2, 2/3, K) 
p 
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h : ù 
11) awe 12)Ss7 13)s-T wf) 15)5sh,p.ù,s) 
é 


4-2 刚体 运动 中 的 全 局 位 置 。 

我 们 将 刚体 坐标 系 B 移动 到 

cd 一 (4 一 3 7)T 

如 果 一 点 的 局 部 位 置 位 于 arp= (7 3 2)T， 试 求 srp。 将 刚体 坐标 系 了 B 绕 着 X 轴 旋 











转 45"， 接 着 绕 着 Y 轴 旋 转 45" 之 后 ， 试 求 刚 体 坐 标 系 B 相对 于 全 局 坐标 系 G 的 方位 。 


4-3 全 局 旋转 和 全 局 变换 。 

一 刚体 绕 着 Z 轴 旋 转 90" 接 着 绕 着 X 轴 旋 转 90°. 

1) 确定 变换 矩阵 5 Tse。 

2) 确定 Brp 二 (1 1 1)7T 在 旋转 之 后 的 全 局 坐标 。 

3) 绕 着 一 特定 的 旋转 轴 旋 转 将 坐标 系 B 变换 到 它 的 新 位 置 ， 请 确定 该 旋转 轴 和 旋转 角 。 
4) 如 果 刚 体 是 处 于 全 局 坐标 系 第 一 象限 的 一 个 立方 体 ， 以 便 点 已 位 于 距 原 点 最 远 的 顶 









































点 处 ， 请 确定 将 立方 体 移 回 到 第 一 象限 顶点 的 所 要 求 的 移动 矢量 “d 。 





5) 将 立方 体 移 回 第 一 象限 之 后 ， 试 确定 点 P 的 全 局 坐标 。 

妈 4-4 ”旋转 矩阵 的 相 容 性 。 

WER rp 、3rp、Sd 给 定 ， 不 可 能 从 下 列 的 刚体 运动 方程 中 求 得 SRB。 
Srp 一 CRBBrp 十 5d 

解释 为 什么 ， 并 给 出 能 够 求 得 “Ra 的 所 要 求 的 条 件 。 

4-5 在 刚体 运动 中 具有 局 部 旋转 的 全 局 位 置 。 

假设 刚体 坐标 系 B 位 于 








GR 04 —=3 DT 





如 果 一 点 的 局 部 位 置 位 于 





Brp=(7 3 2)T 
试 求 Srp 。 将 坐标 系 BSA x 轴 旋 转 45”"， 接 着 围绕 y 轴 旋 转 45 "之 后 求 得 坐标 系 B 相 


对 于 全 局 坐标 系 G 的 方位 。 


转角 





转角 





4-6 局 部 旋转 和 全 局 平 动 。 

刚体 坐标 系 BSE > 轴 旋 转 90"， 接 着 围绕 x 轴 旋 转 90°. 

1) 确定 变换 矩阵 5Ta 。 

2) 确定 矢量 arp=(1 1 1)7 在 旋转 之 后 的 全 局 坐标 。 

3) 绕 着 一 特定 的 旋转 轴 旋 转 将 坐标 系 B 变换 到 它 的 新 位 置 ， 请 确定 该 旋转 轴 和 旋 


























4-7 重复 全 局 旋转 。 

刚体 坐标 系 BSS Z 轴 旋 转 90， 接 着 围绕 Y 轴 旋 转 90°. 

1) 确定 变换 矩阵 5Ta 。 

2) 确定 矢量 arp= (1 1 1)7 在 旋转 之 后 的 全 局 坐标 。 

3) 绕 着 一 特定 的 旋转 轴 旋 转 将 坐标 系 B 变换 到 它 的 新 位 置 ， 请 确定 该 旋转 轴 和 旋 














KO 假设 刚体 是 位 于 全 局 坐标 系 第 一 象限 的 一 个 立方 体 ， 如 图 4-26 HR, A P 是 
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位 于 立方 体 距 原 点 最 远 的 顶点 处 。 证 明 ， : " 
如 果 我 们 三 次 重复 1) 中 的 综合 旋转 ， 那 
么 立方 体 将 返回 至 它 的 最 初 位置 和 方向 。 


KS) 立方 体 绕 着 Z 轴 旋 转 90"， 接 着 绕 
BY 轴 旋 转 90" 之 后 ， 再 绕 着 Su 一 (1 1 (e) 


1T 旋转 4 fA. 6 角 是 多 少时 才能 将 立方 体 
返回 至 它 的 最 初 位 置 和 方向 。 
4-8 刚体 运动 中 的 全 局 旋转 和 局 部 
旋转 。 
将 刚体 坐标 系 也 平移 至 a 
Gd=(4 一 3 7)T 图 4-26 在 全 局 坐标 系 中 处 于 第 一 象限 的 立方 体 
如 果 一 点 的 局 部 位 置 位 于 Brp 二 (7 3 
2)T 时 ， 试 求 crp 。 将 坐标 系 BSA X 轴 旋 转 45"， 接 着 围绕 y 轴 旋 转 45"， 最 后 围绕 > 轴 
旋转 45"， 试 求 坐标 系 B 相对 于 全 局 坐标 系 G 的 方位 。 
4-9 刚体 运动 的 合成 。 
坐标 系 By 围绕 z1 轴 旋 转 35"， 相 对 于 另 一 坐标 系 Bo 再 平移 到 
24 一 (一 40 30 20) 
坐标 系 B， 绕 着 下 列 矢量 轴 旋 转 55"， 试 求 坐标 系 Bs 在 全 局 坐标 系 G 中 方位 。 
u=(2 一 3 4)T 















































计算 sd ACR) 。 

4-10 全 局 旋转 和 全 局 平移 。 

对 于 图 4-26 中 的 单位 立方 体 ， 在 进行 下 列 运动 之 后 求 其 变换 和 矩阵。 

1) RA Z 轴 旋 转 90"， 接 着 沿 着 x 轴 平 移 一 个 单位 。 

2) 在 1) 运动 之 后 ， 确 定点 P 的 全 局 坐标 。 

K3) 重复 1) 中 运动 4 次。 立方体 返回 它 的 最 初 位 置 。 试 确定 每 次 运动 之 后 点 了 的 
坐标 。 

4-11 旋转 在 齐 次 变换 矩阵 中 的 子 和 矩阵 。 

求 齐 次 变换 矩阵 工 中 的 缺失 元 素 。 





? 0 ? 4 
0.707 2? 2? 3 

~ ? 2? 0. 1 
0 0 0 1 


4-12 旋转 角 和 旋转 轴 。 
试 求 工 和 T 一 的 旋转 角 和 旋转 轴 ， 其 中 了 为 








0.866 一 0.5 0 4 
0.5 0.866 0 3 
T= 
0 0 1 1 
0 0 0 1 
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4-13 齐 次 变换 的 合成 。 
假设 坐标 系 Bi 和 坐标 系 Bo 的 原点 分 别 位 于 
2d,;=(—10 20 —20)7 5d 三 (一 15 一 20 30)T 

坐标 系 Bi 绕 着 下 列 矢 量 轴 旋 转 旋转 60°. OR AAA A Bi 在 坐标 系 Bo 中 的 方位 。 
28 一 (1 一 2 4)T 

坐标 系 Bs 绕 着 下 列 矢量 轴 旋 转 旋转 30"， 求 坐标 系 Bo 在 全 局 坐标 系 G 中 的 方位 。 
Gu=(4 3 —4)T 

计算 变换 矩阵 ST，、 T3 STi MISTI 

4-14 绕 着 没有 通过 原点 轴 的 旋转 。 

一 刚体 点 位 于 下 列 矢 量 端点 处 。 
Brp=(7 3 2)T 

绕 着 平行 于 下 列 矢 量 的 一 个 轴 旋 转 60" 且 通过 位 于 (3，3，3) 处 的 一 点 ， 试 求 该 刚体 

点 的 全 局 坐标 。 





























Sy=(4 3 —4)T 

W415 AKA 

图 4-27 所 示 为 两 个 相 邻 的 单位 箱 体 。 矢 量 cwi Su ous EES AP 

1) 将 第 2 个 箱 体 绕 着 xs 旋转 使 其 置 于 第 1 个 箱 体 的 顶部 。 确 定 变换 矩阵 和 4 个 顶点 的 
坐标 。 

2) 将 第 2 个 箱 体 分 别 绕 着 ws 、ws Mu: 旋转 使 其 置 于 第 1 个 箱 体 的 顶部 。 确 定 变 换 矩 阵 
和 4 个 顶点 的 坐标 。 

3) 将 第 2 个 箱 体 分 别 绕 着 Z 轴 和 us 旋转 ， 再 沿 着 Z 轴 平 移 1 个 单位 ， 使 其 置 于 第 1 个 
箱 体 的 顶部 。 确 定 变 换 矩 阵 和 顶点 的 名 称 。 

4) 将 第 1 个 箱 体 分 别 绕 着 ws 旋转 使 其 置 于 第 2 个 箱 体 的 顶部 。 确 定 变 换 矩 阵 和 4 个 顶 
点 的 坐标 。 

5) 将 第 1 个 箱 体 分 别 绕 着 Z 轴 及 
ui1、u3s 和 ws 旋转 使 其 置 于 第 二 个 箱 体 
的 顶部 。 确 定 变 换 矩 阵 和 4 个 顶点 的 
坐标 。 

6) 试 确定 将 第 2 个 箱 体 置 于 第 1 
个 箱 体 顶部 之 上 的 一 个 旋转 和 一 个 
平移 。 

7) 试 确定 将 第 1 个 箱 体 置 于 第 2 
个 箱 体 顶部 之 上 的 一 个 旋转 和 一 个 







































































平移 。 
4-16 Frey. 
已 知 下 列 一 个 2X2 和 矩阵 及 其 道 矩 
阵 为 图 4-27 两 相 邻 单位 箱 体 
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d 0 
ad 十 pc ad +bc 
A l= 
C a 
—ad 十 0c —ad +bc 
利用 求 逆 技术 [ 式 4-90) ] AÉREA FIERET, EE T Hg 
IERE, 
1 2 3 4 
f | 12 13 l4 5 
T= > T= 
C D 11 16 15 6 
10 9 8 7 


W417 绕 着 非 中 心 轴 旋 转 的 合成 。 

考虑 在 全 局 点 (3，0，0) 处 绕 着 一 个 轴 旋 转 30"， 接 着 在 全 局 点 (0，3，0) 处 绕 着 一 
轴 再 旋转 30"。 求 将 矢量 Brp 二 (1 1 0)7T 变 换 到 srp=(V2 0 3)7 的 可 能 变换 矩阵 。 

4-18 刚体 点 的 变换 矩阵 。 

图 4-28a 所 示 为 一 立方 体 的 初始 组 态 。 标 记 立 方 体 顶点 ， 其 最 终 位置 如 图 4-28b 所 示 。 
请 确定 相关 的 齐 次 变换 和 矩阵。 立方 体 的 边 长 是 2。 














图 4-28 刚体 运动 前 后 的 立方 体 











a) 立方 体 初始 组 态 b) 立方 体 最 终 位 置 


4-19 ” 欧 拉 角 和 欧 拉平 动 。 

D 对 于 一 组 欧 拉 旋转 pg、9 和 y AMERRE êo, êo 和 és 分 别 平移 1 个 单位 来 说 ,确定 
齐 次 变换 矩阵 cTp 。 

2) 确定 1) 中 变换 矩阵 的 逆 和 矩阵 。 

3) 对 于 @=45°, O=45°M P=45°, FILO TRAST, |. 

4-20 欧 拉 角 和 全 局 平 动 。 

1) 将 一 个 刚体 绕 着 Su 旋转 45"， 接 着 沿 Sz 平 移 1 个 单位 。 
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Gu 一 (1 2 3)T 

D 确定 齐 次 变换 矩阵 5Tp 。 

3) 确定 欧 拉 旋转 角 p、9 和 yy， 并 且 沿 着 Su 平移 1 个 单位 ， 其 结果 也 有 着 相同 的 STa 。 

4) WA RE YE a KTH ép, ĉo 和 ev 平移 1 个 单位 ， 其 结果 有 着 相同 的 5Ta 。 

D 确定 欧 拉 旋转 角 ep、0 和 yY， 并 且 沿 着 欧 拉 轴 ev ĉo Me 平移 1 个 单位 ， 其 结果 也 
有 着 相同 的 STs。 

4-21 旋转 和 平移 的 错误 次 序 。 

当 我 们 将 一 个 齐 次 变换 分 解 为 一 个 旋转 运动 和 一 个 平移 运动 时 ， 我 们 必须 按照 式 
(4-37) 应 用 第 1 个 旋转 规则 。 





























oTe= GDpSRpA° RE Dz 

D 进行 一 个 旋转 SRz.s* 和 一 个 平 动 YDx.;， 确 定位 于 矢量 Sr 二 (1 1 1)7 处 一 点 的 全 
局 坐标 。 

2) 先 对 矢量 Br 二 (1 1 1)T 平 动 YDx.;， 接 着 旋转 SRz.4s+， 确 定位 于 矢量 Br 处 一 点 的 
全 局 坐标 。 

3) 确定 将 矢量 Sr 移动 到 Sr 所 要 求 的 刚体 运动 。 

4) 确定 将 矢量 Sr 移动 到 Sr 所 要 求 的 刚体 运动 。 

WeA22 改变 旋转 和 平移 的 顺序 。 

当 我 们 将 一 个 齐 次 变换 分 解 为 一 个 旋转 运动 和 一 个 平移 运动 时 ， 我 们 必须 按照 式 (4-37) 
应 用 第 1 个 旋转 规则 。 





GT Tp=°Dp RE Rp DE 
1) 应 用 一 个 常规 的 旋转 “Ra 和 平 动 ?Dsp， 确 定位 于 矢量 Sr 二 (x y xz)T 处 一 点 的 全 
局 坐标 。 
2) 将 矢量 ?7 平移 "De 接着 旋转 “Ra ， 确 定位 于 矢量 sr 处 一 点 的 全 局 坐标 。 
3) 确定 将 矢量 “rm 移动 到 "re 所 要 求 的 转动 和 平 动 。 
4) 引入 一 个 新 的 方程 ， 将 齐 次 变换 矩阵 分 解 为 


GT 二 GD,;GR， GRpS DE 
5) 引入 一 个 新 的 方程 ， 将 齐 次 变换 矩阵 分 解 为 


? 
cTB 一 RD，cRbcDBb 














44-230 主 中 心 螺旋 运动 。 
试 求 将 矢量 arp 移 动 到 srp 的 主 中 心 螺旋 运动 。 
Brp=(1 0 0)T,crp 一 (0 1 4)? 





4-24 螺旋 运动 。 
试 求 将 矢量 Brp 进行 螺旋 运动 5cyB (h, ps ù, s) 一 G3b (4, 60°, a, 8) 之 后 的 全 局 位 置 。 
Brp 一 (1 0 0 1)T,%s=(3 0 0)T,SGu=(1 1 1)7 
K4-25 平面 运动 的 极点 。 
1) 如 果 已 知 两 刚体 点 运动 前 后 的 坐标 ， 试 求 平面 运动 的 极点 位 置 。 
P11,1,1).P2(5,2,1) .Qi1(4,1,1).Q2(7,2+V5 ,1) 
) 证 明 极 点 位 于 直线 PiP: MER QQ 的 交点 处 。 
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D ws: 运动 学 、 动 力学 与 控制 技术 





克 4-26 ”螺旋 参数 。 


刚体 点 Pi. Qi, 








过 一 全 局 点 (0，3，0)， 试 求 刚体 点 Pi, Q Ri 的 全 局 坐标 。 
P1(5,0,0),Q1 (5,5,0), R1 (0,5,0), u= 4 3 —4)T 
利用 这 些 点 的 坐标 计算 与 两 个 旋转 相当 的 螺旋 参数 。 

4-27 非 中 心 旋 转 。 


当 通 过 点 〈 一 1， 一 4，2) 的 刚体 坐标 系 绕 着 cu=(1 1 1)7 旋 转 时 ， 试 求 在 矢量 


Brp=(10 


绕 着 轴 旋 转 30"， 接 着 


20 


Sy=(2 —1 


4-29 


Ri 围绕 x 轴 旋 转 30*"， 接 着 绕 着 平行 于 矢量 Sw 的 轴 旋 转 40"， 并 且 通 





一 10)T 处 点 的 全 局 坐标 。 
让 4-28 等 效 螺旋 运动 。 




















F 行 于 矢量 %d 一 (3 2 一 1)7 平 移 ， 接 着 围绕 着 平行 于 矢量 


H 





DT 的 一 个 旋转 轴 旋 转 40"， 计 算 变 换 和 矩阵 5TaB， 并 求 5Ta 的 螺旋 参数 。 


中 心 螺旋 运动 的 分 解 。 


对 于 附录 C 中 的 第 1 种 情况 ， 
Seth. dg ou ss)=sthx Yo DShy BJ shz cas KR) 


试 求 一 种 三 重 螺旋 运动 ， 使 得 : 


这 里 


友 4-30 





Gsp(h x ûs) =C“ šp (4.606% +s) 


Ss=(3 0 0O)7,u=(1 1 DT 


中 心 螺旋 运动 的 合成 。 


矢量 Brp 二 (10 0 OT 处 一 点 在 经 历 中心 螺 旋 运 动 (4, 30°, J), 接着 (2, 30°, 





* 4-31 





1s» (hı, 


120°, R) 之 后 ， 请 确定 该 点 的 最 终 位 置 。 





螺旋 运动 的 合成 。 
矢量 arp 一 (10 0 OT 处 一 点 先进 行 螺旋 运动 1: 
1/V3 2 
pis Uis 81) =!s5/1, 40°, — 1/43 ， 3 
ie = 
1/9 =3 
Gs, (ho, po» tos So) =6s3,/—1, 45°, [4/9], 1 
4/9 5 


试 求 该 点 的 最 终 位 置 


WA-32 和 零 平 移 的 螺旋 运动 。 








如 果 3 


4-33 





mai 








ES CRIE) AEF, 


TF A SEAR hE 


确定 L 中 的 缺失 数 。 


4-34 
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那么 螺旋 运动 就 变 成 了 绕 着 一 个 非 中 心 轴 的 旋转 运动 。 
试 证 明 对 于 非 中 心 轴 的 旋转 运动 ， 一 个 零 平 移 的 螺旋 运动 相当 于 式 (4-118)。 


tl -1/5 2 4 2- =—1)T 


A HZ 








试 求 局 部 坐标 系 B 中 直线 AE, BE, z 
CE, DE 的 普 吕 克 直 线 ， 计算 图 4-29 所 z 
示 的 四 棱锥 中 AE 5 Z 轴 之 间 的 角度 。 四 
棱锥 棱 边 的 局 部 坐标 : A(1,1,0),B( 一 1， 

1,0),C( 一 1, 一 1,0),D(1, 一 1,0), 瑟 (0， 5 
0,3). 

Ww AHA AE, BE, CE, DE 
转换 到 全 局 坐标 系 G 中 。 原 点 o 的 全 局 位 
置 : oll, 10, 2), 

W435 ”两 线 之 间 的 夹 角 。 

试 求 如 图 4-29 所 示 的 四 棱锥 CEF 
塔 ) 中 OE 和 OD ZMK. RAY AB 图 4-29 四 棱锥 EFR 
标 : D(1, 一 1,0) E(0,0,3)。 

六 4-36 到 原点 的 距离 。 

某 一 平面 方程 为 






































4X —5Y—12Z—1=0 
试 求 原点 到 该 平面 的 垂直 距离 。 
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第 5 章 





对 于 给 定 的 一 组 机 器 人 几何 特征 ， 如 果 知 道 机 器 人 节点 变量 ， 就 能 够 确定 机 需 人 中 每 根 
连 杆 的 位 置 和 方向 。 我 们 可 以 在 每 根 连 杆 上 建立 一 个 坐标 系 ， 并 且 通 过 刚体 运动 方法 来 确定 
相 邻 坐标 系 的 配置 ， 这 样 的 分 析 被 称 为 正 向 运动 学 。 








5.1 D-H 注释 








RA n 个 关节 的 串联 机 器 人 具有 nn 十 1 个 连 E 
杆 。 连 杆 的 数字 从 相对 于 不 可 移动 的 基体 (0) F 
台 ， 依 次 增加 到 末端 执行 器 连 杆 (n)。 对 于 将 第 1 
个 可 移动 的 连 杆 连接 到 基体 连 杆 上 的 关节 来 说 ， 
关节 命名 数字 是 从 1 开始 的 ， 并 依次 增加 到 mw。 因 

















此 ， 连 杆 (i) 通过 关节 i 连接 到 位 于 其 近 端的 下 
一 个 连 杆 上 上， 然后 通过 关节 a 连接 到 位 于 其 远 wl 5-1 F G2) ACCA 














图 5-2 所 示 为 具有 关节 i 一 1、i、i 十 1 的 一 个 
串联 机 器 人 连 杆 (i 一 1)、(i)、(i 十 1)。 每 个 节点 均 由 其 轴 说 明 ， 该 轴 可 以 是 平 动 的， 也 可 
以 是 转动 的 。 为 了 求 得 机 器 人 零 部 件 的 运动 信息 ， 基 于 D-H 标准 方法 ， 我 们 可 以 刚性 地 在 
位 于 关节 i 十 1 的 每 个 连 杆 (i) 上 建立 一 个 局 部 坐标 系 B; 。 

1. zj 轴 与 i 十 1 关节 轴 对 齐 

毫 无 例外 ， 所 有 的 关节 都 可 由 一 个 x 轴 表 述 。 表 述 总 是 从 辨别 z: 轴 开 始 。z; 轴 的 正 向 
方向 是 任意 的 。 对 于 旋转 关节 ， 辨 别 其 关节 轴 是 很 容易 的 ， 然 而 棱柱 关节 〈 即 平 动 关节 ) 可 
以 选择 任何 轴 平 行 于 平 动 方 向 。 也 可 以 通过 对 齐 z; 轴 ， 在 每 个 关节 的 任 一 边 上 的 连 杆 对 ， 
以 及 每 个 连 杆 任 一边 上 的 两 个 关节 来 辨别 。 
虽然 位 于 一 个 连 杆 末端 的 两 个 关节 轴 x; 可 以 是 倾斜 的 ， 但 是 我 们 可 以 使 工业 机 器 人 的 
zi 轴 平 行 、 垂 直 或 者 正 交 。 两 个 平行 关节 可 由 平行 符号 | 说明。 如 果 两 个 关节 轴线 垂直 交 
又 ,那么 这 两 个 关节 轴 是 正 交 的 。 正 交 关 节 轴 由 正 交 符号 上 表示 。 如 果 两 关节 轴 相 对 于 其 公 
共 法 线 是 直角 ， 那 么 这 两 个 关节 是 垂直 的 。 垂 直 关 节 轴 用 垂直 符号 十 表示 。 

2.x; 轴 由 沿 着 zi+1 轴 和 zi 轴 之 间 的 公共 法 线 所 定义 ， 从 zi;-1 轴 指向 z; 轴 

一 般 来 说 ，z 轴 可 以 是 条 和 斜 线 ， 然 而 总 有 一 条 直线 与 其 他 任何 两 条 和 斜 线 相互 垂直 ， 我 们 
称 之 为 公共 法 线 。 公 共 法 线 长 度 是 两 条 斜 线 之 间 最 短 的 距离 。 


(158) 




















图 5-2 具有 坐标 系 B; 和 B;-1 的 连 杆 (i 一 1)、(i)、(i 十 1) 











当 两 个 z 轴 平 行 时 ， 它 们 之 间 有 无 数 个 公共 法 线 。 在 这 种 情况 下 ,我 们 选择 与 前 面 关 
节 的 公共 法 线 共 线 的 直线 作为 公共 法 线 。 

当 两 个 z 轴 相 交 时 ， 它们 之 间 是 没有 公共 法 线 的 。 在 这 种 情况 下 ， 在 zi Xz: 所 确定 
的 方向 上 由 这 两 轴 所 形成 的 平面 ， 建 立 与 该 平面 相 垂直 的 zx; 轴 。 

对 于 两 个 x 轴 共 线 的 情况 ， 关 节 排 列 要 么 是 PR， 要 么 是 R|P。 因 此 ,在 机 器 人 的 
平衡 位 置 ， 我 们 可 以 建立 一 个 使 关节 变量 等 于 零 (9; 二 0) Wr; 轴 。 

3. 用 右手 定 则 确定 yi HH, yi=z xxi 

一 般 来 说 ， 我 们 可 以 将 参考 坐标 系 分配 至 每 根 连 杆 上 ， 以 便 3 个 坐标 轴 e yi zi 中 
的 一 个 坐标 轴 与 远 端 关节 轴 对 齐 。 

通过 应 用 D-H 法 ， 坐 标 系 B，(o;，x;，y;，z;) 中 附加 到 连 杆 G) 的 坐标 原点 o; 可 
以 放置 在 z;-1 轴 与 x; 轴 之 间 公 共 法 线 与 关节 i 十 1 的 交叉 点 上 。 

D-H 坐标 系 可 由 a;、a;、0;、d; 这 4 个 参数 来 识别 。 

D 连 杆 长 度 a; 是 指 沿 着 x; 轴 的 xz;-1 轴 和 >; 轴 之 间 的 距离 。a; 是 连 杆 G) 的 运动 
长 度 。 

2) 连 杆 扭 ( 旋 ) 转角 a; 是 z;-1 轴 绕 着 x; 轴 所 转动 的 角度 ， 以 使 z;-1 轴 平行 于 zx; 轴 。 

3) 关节 距离 d; 是 指 沿 着 >;_1 轴 的 zx;-1 轴 和 zx; 轴 之 间 的 距离 。 关 节 距 离 也 称 为 连 杆 偏 
置 值 。 

4) 关节 角 0; 是 x;-1 轴 绕 着 z;-1 轴 所 转动 的 角度 ， 以 使 x;-1 轴 平行 于 x; 轴 。 

如 图 5-2 所 示 的 连 杆 D-H 坐标 参数 在 图 5-3 中 进行 了 标注 。 参 数 90; Ad: 被 称 为 关节 参 
数 ， 因 为 它们 确定 了 由 关节 7 所 连接 的 两 相 邻 连 杆 的 相对 位 置 。 在 机 器 人 设计 中 ， 每 个 关节 
都 是 旋转 的 或 者 平移 的 。 因 此 ， 对 于 每 个 关节 ， 参 数 0; 或 参数 di 是 固定 的 ， 其 他 参数 是 可 
对 于 关节 i MRK TR, BSA di 的 值 是 不 变 的 ， 而 参数 0; 是 唯一 的 关节 变量 。 

变 参 数 9; Mdi 被 称 为 关节 变量 。 关 节 参 数 9; Md; 定义 螺旋 和 运动， 因为 参数 0; 是 绕 着 
We 参数 4; 是 沿 着 >;_1 轴 的 一 个 平 动 位 移 。 
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ws: 运动 学 、 动 力学 与 控制 技术 





参数 a; 和 a; 被 称 为 连 杆 参数 ， 因 为 它们 定义 了 连 杆 G) 两 个 末端 处 的 关节 i 和 关节 
i 十 1 的 相对 位 置 。 连 杆 旋 转角 a; 是 x;-1 轴 绕 着 x; 轴 的 旋转 角度 ， 以 使 >;-1 轴 平行 于 xz; 轴 。 
EIKE a; 是 沿 着 x; 轴 平 移 的 距离 ， 以 使 x;-1 轴 和 x; 轴 重 合 。 连 杆 参数 a; 和 a; 定义 一 
个 螺旋 运动 ， 因 为 参数 a; 是 绕 着 x; 轴 的 一 个 旋转 角 ， 参数 a; 是 沿 着 x; 轴 的 一 个 平 动 
位 移 。 























图 5-3 ”对 于 关节 ;和 连 杆 G) 所 定义 的 D-H 参数 a;、a;、0; Mdi 








换 句 话说 ， 通 过 中 心 螺旋 运动 x (Ca;. ais 1) 将 z;_1 轴 移动 至 x; Hh, 通过 中 心 螺旋 运 
Bhs Cdi, Ois kii) 将 x;-1 轴 移动 至 x; 轴 。 

例 5-1 D-H 法 的 简短 说 明 。 

下 面 有 简化 D-H 法 应 用 的 一 些 说 明 . 


























1) 仅仅 说 明 > 和 x 轴 便 可 充分 地 辨识 一 个 坐标 系 。 不 需要 展示 y 轴 ， 就 可 清楚 地 绘制 
坐标 系 。 
2) 如 果 第 1 个 和 最 后 一 个 关节 是 旋转 关节 R， 则 有 
ao =Q, an 一 0 (5-1) 
ao =0, a, =0 (5-2) 
在 这 些 情况 中 ， 对 于 0 AO, 的 零 位 置 可 以 任意 选择 ， 连 杆 偏 置 值 可 以 置 为 零 。 
d,=0.d,=0 (5-3) 
3) 如 果 第 1 个 和 最 后 一 个 关节 是 棱柱 关节 CREAR) P, WA: 
01 二 0,0, = (5-4) 
对 于 di Ad, 的 零 位 置 可 以 任意 选择 ,但 是 我 们 通常 选择 di Ad, 以 使 更 多 参数 尽 可 能 





4) 如 果 最 后 关节 是 旋转 关节 R, H 0, =0 时 我 们 选择 zx， 轴 使 其 对 齐 zx-1 轴 ， 坐 标 系 
Br 的 原点 可 以 选择 ， 以 使 d; 一 0。 如 果 最 后 关节 nn 是 棱柱 关节 了 P， 我 们 选择 zx， 轴 使 2 一 0， 
坐标 系 By 的 原点 位 于 zz 一 1 轴 和 关节 n 的 相交 点 处 。 

5) 除了 基 座 和 最 后 一 个 连 杆 外 ， 每 个 连 杆 都 是 一 个 二 进 制 链接 ， 被 相应 地 连接 到 两 个 
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其 他 连 杆 上 。 

6) 机 器 人 几何 参数 设计 决定 了 参数 a; 和 参数 a;， 它 们 总 是 常数 。 距 离 di 是 坐标 系 Bi 
相对 于 坐标 系 B,-1 沿 着 z;-1 轴 的 偏 移 量 。 因 为 a; 表示 长 度 ， 因 此 a: 20, 

7) 角度 a; 和 4 是 定向 的 。 分 别 依据 z; 轴 和 xz;--1 轴 的 方向 ， 角 度 的 正 向 由 右手 定 则 确定 。 

D 对 于 工业 机 器 人 而 言 ， 连 杆 旋 转角 a; 通常 是 x/2 的 整数 倍 。 

9) D-H 坐标 系 并 不 是 唯一 的 ， 因 为 z; 轴 的 方向 是 任意 的 。 

10) 对 于 固定 机 器 人 来 说 ， 基 体 坐 标 系 Bo (xo ,yo ,zo) 二 全 局 坐标 系 G(X,Y,Z)。 沿 着 
关节 1 的 轴线 可 方便 地 选择 Z 轴 ， 在 G 坐标 系 坐 标 轴 与 Bi 坐标 系 的 坐标 轴 共 线 或 者 平行 的 
地 方 选择 坐标 原点 O. 

11) 所 有 关节 变量 均 为 零 的 机 器 人 配置 称 为 平衡 位 置 或 者 起 始 配置 ， 对 于 机 器 人 的 所 有 
运动 ， 它 是 一 个 参数 。 最 好 的 平衡 位 置 是 许多 轴 尽 可 能 地 彼此 平行 并 且 共 面 。 

12) 为 了 方便 ， 对 于 x; 轴 的 方向 我 们 可 以 放宽 严格 的 D-H 定义 ， 以便 zx; 轴 的 方向 从 
zi 轴 指 向 zx;-1 轴 ， 并 且 仍 然 获得 有 效 的 D-H 参数 。 当 大 部 分 关节 参数 为 零 时 ，x; 轴 的 方向 
性 旋转 将 更 加 方便 地 设置 参考 坐标 系 。 

13) D-H 参数 表 有 助 于 建立 一 个 系统 的 连 杆 坐标 系 ， 见 表 5-1。D-H 参数 表 对 于 坐标 系 
参数 有 5 列 和 4 个 D-H 参数 。 对 于 每 个 坐标 系 的 4 个 D-H 参数 的 行将 由 常数 和 关节 变量 填 
充 ， 连 杆 将 随 着 每 个 变化 着 的 关节 而 移动 。 

表 5-1 连 杆 坐标 系 的 D-H 参数 
























































坐标 系 编号 aj Qi di 0; 
1 al Ql dı 01 
2 az az dz 02 
j aj aj dj 9; 
n an an d n On 














例 5-2 3R 平面 机 械 手 的 D-H 参数 表 及 坐标 系 。 








N 
= 
5 

K 
= 
= 
= 
> 
eN 
> 
i 
> 





和 T 的 D-H 坐标 系 
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R 代表 旋转 关节 ， 因 此 有 1 RI R 机 械 手 就 是 一 个 具有 3 个 平行 旋转 关节 的 平面 机 器 人 。 
图 5-4 所 示 为 一 个 3R 平面 机 械 手 。 所 填写 的 D-H 参数 见 表 5-2， 所 建立 的 连 杆 坐标 系 在 图 














5-4 中 进行 了 标示 。 
表 5-2 5-4 中 3R 平面 机 械 手 的 D-H 参数 


























坐标 系 编号 ai ai d; 0; 
1 Lı 0 0 01 
2 ls 0 0 0» 
3 ly 0 0 03 











例 5-3 3R PUMA 机 械 手 的 坐标 系 。 





如 图 5-5 所 示 的 PUMA 机 械 手 ， 有 R 上 RR 旋转 关节 ， 如 果 和 忽略 机 械 手 末端 执行 器 的 
结构 ， 附 加 到 机 械 手 连 杆 上 的 坐标 系 在 图 中 进行 了 标示 ， 其 D-H 参数 见 表 5-3。 
RRR 机械 手 的 关节 轴 被 称 为 机 身 z。、 户 部 zi 和 肘 部 =* 。 典 型 地 ， 关 节 轴 1 MK 




















节 轴 zs 是 相互 平行 的 ，z1 和 zs 垂直 于 zo。 








zi 





图 5-5 3R PUMA 机 械 手 及 其 连 杆 坐标 系 








表 5-3 5-5 所 示 的 3R PUMA 机 械 手 的 D-H 参数 
坐标 系 编 号 Qi a; di 


























0; 
1 0 90° 0 01 
2 l2 0 li 02 
3 0 —90° 0 03 


例 5-4 斯 坦 福 机 械 手 。 

















加 了 一 个 R 上 FR 上 FR 关节， 
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斯 坦 福 机 械 手 的 原理 性 说 明 如 图 5-6 所 示 ， 它 是 一 个 球 坐 标 机 器 人 RFR 上 FPP， 手腕 上 附 

















图 5-6 斯坦福 机 械 手 的 原理 性 说 明 








表 5-4 5-6 所 示 的 斯 坦 福 机 械 手 的 D-H 参数 




















坐标 系 编号 ai a; d 0; 
1 0 —90 li 01 
2 0 90 le 0 
3 0 0 d3 0 
4 0 —=90 0 04 
5 0 90° 0 05 
6 0 0 Ls Os 




















斯 坦 福 机 械 手 的 D-H 参数 见 表 5-4。 该 机 械 
手 有 6 个 自由 度 ， 分 别 为 : G1. 02, d3, G4, 05 
和 06 。 

例 5-5 特殊 的 坐标 系 。 

在 机 械 手 中 ， 一些 坐标 系 有 着 特殊 的 名 字 。 

基体 坐标 系 Bo 或 者 G 安装 机 器 人 的 基体 连 
杆 。 在 基体 坐标 系 中 ， 必 须 计 算 每 个 运动 信息 ， 
因为 末端 执行 器 的 起 始点 、 运 动 路 径 和 到 达 点 都 
要 在 基体 坐标 系 中 进行 定义 。 基 体 坐 标 系 和 工具 
坐标 系 如 图 5-7 所 示 。 











图 5-7 








基体 坐标 系 和 工具 坐标 系 


基体 坐标 系 (station frame) S 也 被 称 为 世界 坐标 系 (the world frame) 或 者 通用 坐标 
系 (universe frame), eA PLA AGS EA BRR SULLA A RE AE, 或 者 
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当 机 器 人 是 移动 的 时 ， 基 体 坐 标 系 是 非常 重要 的 。 

手腕 坐标 系 (wrist frame) W 在 手腕 处 建立 。 手 及 就 是 将 末端 执行 器 连接 至 机 器 人 最 
后 一 个 手臂 的 环节 。 工 业 机 器 人 机 械 手 通常 连接 至 具有 3 个 正 交 旋转 轴 的 手 脐 机 构 上 。 

工具 坐标 系 (tool frame) T 建立 在 机 器 人 夹 持 工具 的 末端 处 。 它 也 可 以 被 称 为 末端 执 
行 器 坐标 系 (end-effector frame) 或 者 终端 坐标 系 (final frame)。 当 机 械 手 是 空 的 时 ， 末 并 
执行 器 坐标 系 通常 选择 在 机 器 人 手 尖 之 间 的 原点 处 。 工 具 坐 标 系 如 图 5-7 Bran. 

目标 坐标 系 (goal frame) F 位 于 机 器 人 移动 的 工具 上 。 相 对 于 基体 坐标 系 确定 目标 坐 
标 系 。 

例 5-6 将 坐标 系 Bii 变换 至 坐标 系 Bio 

两 相 邻 坐标 系 可 以 由 一 系列 平 动 和 转动 的 序列 进行 转化 。 下 面 是 预 设 两 个 旋转 和 两 个 平 
动 的 集合 是 一 个 移动 坐标 系 B;-1， 以 便 与 坐标 系 B, 一致 的 直接 方法 。 

1) 沿 着 坐标 轴 z;-1 平 移 坐 标 系 Bi FEES di. 

2) 绕 着 坐标 轴 z;-1 旋 转 坐 标 系 B;-1， 旋 转角 度 0; 。 

3) 沿 着 坐标 轴 x; 平移 坐标 系 B;-1， 平 移 距 离 ai. 

4) 绕 着 坐标 轴 zx; 旋转 坐标 系 B;-1， 旋 转角 度 a; 

然而 ， 为 了 求 得 变换 矩阵 ， 在 坐标 系 Bi- ,的 当前 配置 下 ， 我 们 应 该 从 两 个 相互 一 致 的 坐 
标 系 B: 和 B;-1 开 始 ， 将 坐标 系 Bi 移动 至 当前 位 置 。 因 此 ， 我 们 必须 遵守 下 面 的 运动 次 序 。 

1) 绕 着 坐标 轴 zx;-1 旋 转 坐 标 系 B; ， 旋 转角 度 a; 。 

2) 沿 着 坐标 轴 zx;-1 平 移 坐 标 系 B; PRES ai. 

3) 绕 着 坐标 轴 x;-1 旋 转 坐 标 系 B; ， 旋 转角 度 0; 。 

4) 沿 着 坐标 轴 z;-1 平 移 坐 标 系 Bi PEES di. 

在 这 些 运动 演变 期 间 ， 坐标 系 B;-1 相 对 于 局 部 坐标 系 B; 充当 着 全 局 坐标 系 ， 这 些 运动 
是 绕 着 和 沿 着 全 局 坐标 轴 的 。 

例 5-7 D-H 法 的 缺点 。 
用 于 描述 连 杆 坐标 系 的 D-H 法 既 不 是 唯一 的 方法 ， 也 不 是 最 好 的 方法 。D-HH 法 的 缺点 如 下 : 

1) 连续 坐标 轴 以 这 样 的 方式 定义 : 坐标 原点 of 和 坐标 轴 xz; 定义 在 同时 垂直 于 相 邻 连 
杆 轴 上 。 这 是 一 个 取决 于 连 杆 几何 形状 的 困难 任务 ， 并 且 会 产生 奇异 性 。 

2) D-H 法 并 不 能 扩展 到 三 元 连 杆 和 复合 连 杆 中 。 






















































































5.2 相 邻 两 坐标 系 间 的 变换 


坐标 系 B; 固定 于 连 杆 (i) E, BERR B;-1 固 定 于 连 杆 (i 一 1) 上 。 基 于 D-H 法 的 规 
定 ， pe, 变换 至 坐标 系 Bi-1 的 变换 矩阵 一 1T; 可 以 用 连 杆 (i) MRI i AY 4 MER 








变换 的 乘积 
cos0; —sinĝ; cosa; sind ; sina; a;cosd; 
sind; cos0;cosa; —cosĝ;sina; a;sind; 
i TSD 225 ,aR oD a aR ia = F 
0 sina ; cosa; d; 
0 0 0 1 
(5-5) 
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1 0 0 0 
0 cosa; —sina; 0 
及 -ui = (5-6) 
0 sine; cosa i 0 
0 0 0 1 
L © O a3 
0 1 0 0 
Dei ,au， = (5-7) 
0 0 1 0 
0 0 0 1 
cos0; —sinĝ; 0 0 
sing; cosĝ; 0 0 
R. 6; 一 (5-8) 
0 0 1 0 
0 0 0 1 
1 0 0 a; 
0 1 0 0 
D.,., 1,4; = (5-9) 
0 0 1 d; 
0 0 0 1 


因此 ， 从 坐标 系 B: (is yis zi) 变换 到 它 前 一 个 坐标 系 Bii (zi-1， yi-1- zi-1) 
的 变换 方程 为 





Xi—l Xi 
Ji 一 1 . yi 
= !7; (5-10) 
之 i 一 1 Zi 
1 1 
这 里 
cosl; —sing;cosa; sing; sina; a;cosd; 
; sind; cosl; cosa; —cos@;sina; a;sind; 
a = (5-11) 
0 sina ; COSa@ ; di 
0 0 0 1 


这 个 4X4 FET WA Be el GP ke 4S RE. 80 DS EE Sa AG Re Ph BY Tn FR a 
JA 7 She AOR ie oh, (ED SER AE MAB ip A Bi (xis yis zi) 到 坐标 系 
Bia (zi-1，yi-1，zi-1) 所 要 求 的 相同 的 刚性 运动 。 











i—1R, ‘lq; 
一个; 一 (5-12) 
0 1 
这 里 
cosl; —sind;cosa; sing; sina; 
‘~1R;=| sind; cosĝ; cosa ; — cos; sina; (5-13) 
0 sina ; cosa; 
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D 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 









































a;cosd; 
‘~ld;=| aisin0， (5-14) 
dj 
齐 次 变换 矩阵 式 〈5-11) BB EE 
cosĝ; sind; 0 一 Qi; 
maciri 一 Sin0icosa cosĝ;cose; sina; —d;sina; ee aes 
sing; sina; —cos0;sina; cosa; —d;cosa; 
0 0 0 1 
证 明 1: 
假设 图 5-8 中 的 坐标 系 Bo (£2, yo. z2) 和 坐标 系 B (zi; yi; z1) 是 按照 D-H 规 
则 建立 的 。 
i 
图 5-8 Æ F D-H 法 的 两 个 坐标 系 
利用 矢量 ?rp 和 is。， 点 卫 的 位 置 矢 量 在 坐标 系 Bl is yis z1) 可 以 表述 为 
lr p=!R} rp +! ss (5-16) 
其 齐 次 坐标 表述 形式 为 
xı cos(i2,21) cos(j2,11) cos(R2,21) s2, Le 
y1 | a Sosa) costa a s2, || y2 Gan 
之 1 cos(i2,Rk1) cos(j2+ki) cos(k2,ki) s2, || 之 2 
l 0 0 0 Lys 
使 用 图 5-8 中 所 标注 的 参数 ， 式 (5-17) 可 变 为 
Xl1 cło 一 SO cay， s02 sa? azch2\ [x2 
yı s02 c2 caz —cO2caz a2802 || y2 
之 1 E 0 sa? caz do 之 2 
1 0 0 0 1 1 
如 果 用 坐标 系 B;_1 (zx;-1，yi;-1，zi-1) 替换 坐标 系 Bl ， 用 坐标 系 Bi is yis zi) 











替换 坐标 系 B: ， 则 可 以 将 上 述 方程 重 写成 所 要 求 的 形式 : 
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XLi-1 c0; 一 SOi ca; s; sa; arc; Xi 
= sO; chica: —cOica; a2sOi i 
daas i ed ie (5-19) 
之 ;1 一 1 0 SQ 2 Ca; d; Zi 
1 0 0 0 1 1 
依据 齐 次 变换 矩阵 的 逆 规 则 ， 有 
GR, Gs 
i—IT; = (5-20) 
0 1 
GRE —SRISs 
or E B (5-21) 
0 1 
我 们 也 可 以 求 得 所 要 求 式 (5-15) 的 道 变 换 。 
Xi c0; s0; 0 一 Qi Til 
i — sl; ca ch; ca; se? —disai J= 
TE tains al (5-22) 
Zi sOisa;  —CO;ca; ca; 一 dicai || zi-1 
1 0 0 0 1 1 


证 明 2: 

求 取 'T 了 ;1 的 男 一 种 可 替代 的 方法 是 按照 平 动 和 转动 序列 将 坐标 系 Bi- 变换 至 当前 的 坐 
标 系 B; 。 这 两 个 坐标 系 也 可 以 由 B 志 9B; 和 G 志 B;-1 进 行 等 效 描述 ， 因 此 所 有 紧 接 着 的 转 
动 和 平 动 是 绕 着 和 沿 着 局 部 坐标 轴 而 形成 的 。 观 察 图 5-8 可 知 : 

D 坐标 系 B;--1 沿 着 局 部 坐标 轴 z; 平移 ， 平 移 距 离 为 一 d;。 

2) 被 移动 的 坐标 系 B;-1 绕 着 局 部 坐标 轴 z; 旋转 ， 旋 转角 度 为 一 0;。 

D 被 移动 的 坐标 系 B;-1 沿 着 局 部 坐标 轴 zx; 平 动 ， 平 动 距 离 为 一 a;。 

4) 被 移动 的 坐标 系 B;-1 绕 着 局 部 坐标 轴 x; 旋转 ， 旋 转角 度 为 一 a; 。 

按照 这 些 移动 序列 ， 变 换 和 矩阵 :了 T;-1 为 
































cosd ; sind; 0 —a; 
—sinO;cosa; cosd;cosa; sina; —d;sina; 
T: SR. aiD.., aes, 0 也 -， di 一 
sind ; sina; —cos8;sina; cosa; —d;cosa; 
0 0 0 1 
(5-23) 
这 里 
1 0 0 0 
0 1 0 0 
D.,,-a,; = (5-24) 
0 0 1 一 di; 
0 0 0 1 
cos?; —sinĝ; 0 0 
sind; cos6; 0 0 
R.,,-6, = (5-25) 
0 0 1 0 
0 0 @ 1 
外” 三 表示 等 效 于 ， 全 书 同 。 
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1 0 0 aj 
0 1 0 0 
D.,,-a; = (5-26) 
0 0 1 0 
0 0 0 1 
1 0 0 0 
0 cosa; —sina; 0 
Rozsa” . (5-27) 
0 sine; COSQ i 0 
0 0 0 1 
AJT: =T; ADORE 
cos; —sinĝ; cosa; sind ; sina; a;cosd; 
, i sind; cosl; cosa; —cosé;sina; a;sind; 
17, =T; = . (5-28) 
0 sina ; COSa ; d; 
0 0 0 1 


例 5-8 2R 平面 机 械 手 的 D-H 变换 矩阵 。 








图 5-9 2R 平 面 机 械 手 和 每 根 连 杆 的 D-H 坐标 系 





5-9 所 示 为 2R(R || R) 平面 机 械 手 及 其 D-H 连 杆 坐标 系 。 其 D-H 参数 见 表 5-5。 
表 5-5 5-9 所 示 2R 平面 机 械 手 的 D-H 参数 











坐标 系 编号 ai ai di 0; 
1 li 0 0 0i 
2 lz 0 0 02 




















基于 表 5-5 所 列 的 D-H 参数 ， 我 们 可 以 通过 直接 取代 式 (5-11) 中 的 D-H 参数 求 得 从 
坐标 系 B; 到 坐标 系 B;_; 的 变换 和 矩阵。 因此， 有 











cos0» 一 Sin0， 0 /cosO， 
Sin0> cos0» 0 /»sin0» 
IT; 一 (5-29) 
0 0 1 0 
0 0 0 1 
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cosO1 —sind; 0 /icosOl 
sing | cos 0 isini 
oT 一 (5-30) 
0 0 1 0 
0 0 0 1 
因此 ， 有 
c(01ı +02) s(Q, +02) 0 lic lL2c(01 +02) 
SCO1 +02) c(01ı +02) 0 ls l2s(01ı +02 
°T, =°T T = (5-31) 
0 0 1 0 
0 0 0 1 





例 5-9 HAR|RKARI|P 
关节 的 连 杆 。 

HEFT G) 的 棱柱 关节 是 旋转 
的 ， 并 且 当 末端 关节 是 旋转 的 或 者 
平 动 的 ， 同 时 在 两 个 末端 处 的 关节 
轴 是 平行 的 时 (如 图 5-10 所 示 )， 则 
ai=0° (RA a; =180°), a; 是 两 个 
关节 轴 之 间 的 距离 ，0; 是 唯一 的 可 
变 参数 。 关 节 距 离 di 二 常量 且 是 沿 
着 zi 轴 的 坐标 系 B; 和 坐标 系 Bi- 
之 间 的 距离 。 然 而 我 们 通常 假设 
Xiyi 和 zx;-1y;-1 共 面 ， 使 得 d; =0. 
对 于 静止 位 置 上 RR 的 连 杆 来 说 ， 
坐标 轴 x; 和 x;-1 是 平行 的 。 

因此 ， 对 于 具有 a; 二 0 以 及 RR 












































R 或 者 RWHP 关节 的 连 杆 来 说 ， 变 换 图 5-10 平行 关节 RR (09 的 连 杆 
REIT: 为 
cos0; —sinO; 0 a;cosd; 
sind; cos; 0 a;sind; 
at ie (5-32) 
0 0 1 d; 
0 0 0 il 
与 此 同时 ， 对 于 具有 a; 二 180" 以 及 R || RMA R || P AEREI, ERE IT, 为 
cosl; sind; 0  a;cosd; 
l sinf; — cos; 0 aisinĝ; 
1 人 个; 一 (5-33) 
0 0 =] di 
0 0 0 1 


例 5-10 具有 FR]R 或 者 RLP 关节 的 连 杆 。 

当 连 杆 (i) 的 棱柱 关节 是 旋转 的 ， 并 且 当 末端 关节 是 旋转 的 或 者 平 动 的 ， 同 时 在 两 个 末端 
处 的 关节 轴 是 垂直 的 时 ， 如 图 5-11 所 示 ， 则 a; =90° (或 者 ai =—90°). a; 是 坐标 轴 x; 上 两 个 关 
节 轴 之 间 的 距离 ，0; 是 唯一 的 可 变 参数 。 关 节 上 距离 di 二 常量 且 是 沿 着 zx; 轴 的 坐标 系 B: 和 坐标 系 
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Bi-1 之 间 的 距离 。 然 而 我 们 通常 假设 ziy， 
Al zi-iyi-i Ft. 1879 di =0. 

RA R || R 连 杆 的 中 心 线 坐 标 轴 
Xi-1 旋 转 90°, WI RLR 连 杆 。 对 于 
静止 位 置 上 的 人 1LR 连 杆 , 坐标 轴 a: 
和 坐标 轴 zx;--1 是 平行 的 。 

因此 ， 对 于 具有 a; =90°F RR BR 
J 上 LP 关节 的 连 杆 来 说 ,例如 RLR (90°) 
或 者 RLP (90)， 其 变换 矩阵 一 1 为 




















cos6; sind; a;cosd; 


0 
= sind; 0 —cosé; a;sind; 
四 1 

0 








0 0 di 
0 0 1 
(5-34) 
与 此 同时 ， 对 于 具有 a; = — 90° 图 5-11 垂直 关节 RRR (90°) 的 连 杆 

















及 RLR 或 者 RLP 关节 的 连 杆 来 说 ， 
Hlan RLR (—90°) 或 者 RLP (一 90")， 变 换 和 矩阵 和 1T; 为 





cosĝ; 0 —sind; aicosO; 
sind; 0 cosO a; sind; 
i—IT; = (5-35) 
0 =] 0 d; 
0 0 0 1 


$l 5-11 ALA R ER ae RFP 关节 的 连 杆 。 

当 连 杆 G) 的 棱柱 关节 是 旋转 的 ， 并 且 当 末端 关节 是 旋转 的 或 者 平 动 的 ， 同 时 在 两 个 末 
端 处 的 关节 轴 是 平行 的 时 ， 如 图 5-12 所 示 ， 则 a; =90° (或 者 w =—90°), ai=0, 4d; 一 常量 且 
是 坐标 轴 z; 上 两 个 坐标 原点 之 间 的 距离 ，0; 是 唯一 的 可 变 参数 。 或 者 假设 di =0 是 有 可 能 的 。 
位 于 静止 位 置 的 R FR 连 杆 ， 当 di AO 时 ， 坐 标 轴 zx; 和 坐标 轴 x;-1 是 重合 或 者 平行 的 。 

因此 ， 对 于 具有 a; 一 90" 和 R 上 R 或 R 上 FP 关节 的 连 杆 来 说 ,例如 R 上 FR (90°) 或 者 RI 上 
P (90")， 其 变换 和 矩阵 生 1T; 为 


























cosĝ; 


人 ;一 (5-36) 


0 

0 

与 此 同时 ， 对 于 具有 a; 二 一 90" 以 及 R 上 上 R 或 者 R 上 FP 关节 的 连 杆 来 说 ,例如 R ER 
(—90°) 或 者 RR 上 FP (一 90") ， 变 换 矩 阵 一 1T; 为 


0 0 

sind; 0 一 cos0O 0 
1 d 
0 





cos; 0 —sind; 0 

sind; 0 cos; 0 
“17; = (5-37) 

0 —] 0 d; 

0 0 0 1 
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例 5-12 具有 P|R 或 者 PP 关节 的 





HEF Gi) 的 棱柱 关节 是 旋转 的 ， 并 
且 当 末端 关节 是 旋转 的 或 者 平移 的 ， 同 时 在 
两 个 末端 处 的 关节 轴 是 平行 的 时 ， 如 图 5-13 
Bras, Wa; =0 (或 者 a; 二 180 )，0 一 0 ， 
a; 是 坐标 轴 x; 上 两 个 坐标 原点 之 间 的 距 
Š, di 是 唯一 的 可 变 参 数 。 注 意 ， 此 时 ， 


























ai =0 是 有 可 能 的 。 


Alt, FARA a; =0 AP || RK 
PP 关节 的 连 杆 ， 例 如 Pl ROD 或 者 P| 
P(0") ， 其 变换 矩阵 一 1T， 





1 0 
= 0 1 
i t= 
0 0 

0 0 





为 
0 


Co = © 





图 5-12 垂直 关节 RFR (90°) 的 连 杆 
(5-38) 


与 此 同时 ， 对 于 具有 a; =180WURK P| R 或 者 P|P 关 节 的 连 杆 来 说 ， 例 如 了 P || RC180°) 
或 者 P|P(180") ， 变 换 和 矩阵 一 IT 为 


1 0 0 ai 
Oi == 0 0 
A = (5-39) 
0 0 =1 ds 
0 0 0 1 





Al 5-13 ”平行 关节 P || ROO?) 的 连 杆 
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坐标 系 B;-1 的 原点 可 以 选择 在 zi 坐标 
轴 上 的 任何 一 点 处 ， 或 者 选择 在 平行 于 zi-1 坐 
标 轴 上 的 任何 一 点 处 。 一 个 简单 的 设置 就 是 将 
平 动 棱柱 关节 的 原点 of 设置 在 前 一 个 坐标 系 
的 原点 oj-1 处 。 因 此 ， 这 使 得 ai =O 以 及 关节 
变量 d; 的 初始 值 为 零 di 二 0， 当 坐标 原点 0; 
沿 着 平行 于 坐标 轴 z;-1 上 下 滑动 时 ，a; 和 <， 


























也 随 之 改变 ， 
例 5-13 具有 PLR 或 者 PLP 关节 的 
连 杆 。 


“MER G) 的 棱柱 关节 是 旋转 的 ， 并 且 
当 末 端 关节 是 旋转 的 或 者 平移 的 ， 同 时 在 两 个 
末端 处 垂直 的 关节 轴 时 ， 如 图 5-14 所 示 ， 则 
ai=90° (或 者 a; =— 90°), 0;=0, ai 是 坐标 
轴 xz; 上 两 个 关节 轴 之 间 的 距离 ，4; 是 唯一 的 图 5-14 垂直 关节 PJ R(90") 的 连 杆 
可 变 参数 。 

因此 ， 对 于 具有 wj; 一 90" 和 了 PR 或 者 PP 关节 的 连 杆 ， 例 如 P 1 R(90") 或 者 P|] P 
(90°), ， 其 变换 矩阵 一 1T; 为 


























1 0 0 a; 
0 0 =I 0 
“Ts (5-40) 
0 1 0 d; 
0 0 0 1 





与 此 同时 ， 对 于 具有 ea = 一 90" 以 及 PR 或 者 PLP 关 节 的 连 杆 来 说 ， 例 如 PR 
(—90°) 或 者 PP( 一 90")， 变 换 矩 阵 二 17T; 为 





1 0 0 Ai 

I 0 0 1 0 

IT, = (5-41) 
0 一 1 0 d; 
0 0 0 1 


例 5-14 AA PHR 或 者 PHP 关节 的 连 杆 。 

当 连 杆 (i) 的 楼 柱 关节 是 旋转 的 ， 并 且 当 末端 关节 是 旋转 的 或 者 平移 的 ， 同 时 在 两 个 
末端 处 的 两 个 关节 轴 是 相 垂直 的 时 ， 如 图 5-15 所 示 ， 则 a; =90° (或 者 w = 一 90") ，0; 一 0， 
a;=0, di 是 唯一 的 可 变 参 数 。 注 意 ，x; 必须 垂直 于 坐标 轴 z;_1 和 坐标 轴 x; 所 在 平面 ， 此 
时 有 可 能 ai %0. 

因此 ， 对 于 具有 a; 一 90" 和 PHR RA PEP 关节 的 连 杆 ， 例 如 PER(90°) 或 者 PHP 
《90 )， 其 变换 矩阵 一 1T; 为 


















































1 0 0 a 
0 0 =L 0 
“IT; = (5-42) 
0 1 0 di 
0 0 0 1 
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图 5-15 ”相交 垂直 关节 PHR(90") 的 连 杆 





对 于 具有 a; =—90° WR PER 或 者 PHP 关节 的 连 杆 来 说 ， 例 如 和 PERC—90°) 或 者 了 
EP(—90°), FEAR FEET; 为 











1 0 0 0 

l 0 0 1 0 

—1T;= (5-43) 
0 一 1] 0 di 
0 0 0 1 


例 5-15 ”装配 工业 连 杆 ， 制 作 一 个 机 械 手 。 

通常 通过 连接 例 5-9 一 例 5-14 中 所 介绍 的 工业 连 杆 制作 工业 机 械 手 。 机 械 手 是 3 个 连 杆 的 
合成 ， 对 于 笛 卡 儿 空 间 中 的 一 个 点 ， 这 3 个 连 杆 可 提供 AARE, BE BERN BRIG WL IME A FF JL 
的 实际 机 械 手 。 图 5-16a 和 图 5-16b 所 示 表 明 如 何 通过 连接 恰当 的 工业 连 杆 制 作 这 些 机 械 手 。 

















a) b) 


图 5-16 BCH HI ERIE WLI F a A IL A SE PROLIF 
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例 5-16 ”工业 机 器 人 连 杆 的 分 类 。 
通过 机 器 人 连 杆 两 端的 关节 辨别 机 器 人 连 杆 ， 机 器 人 连 杆 决 定 了 从 末端 关节 坐标 系 Bi 
到 近 端 坐标 系 B;-1 的 变换 和 矩阵。 制作 工业 机 器 人 有 12 种 类 型 的 连 杆 。 每 种 类 型 的 变换 矩阵 
唯一 取决 于 z 轴 之 间 的 近 端 关节 和 角度 。 连 杆 的 12 种 关节 类 型 见 表 5-6. 
表 5-6 ， 连 杆 的 关节 类 型 






























































1 R || RCo?) BK R || PCO?) 7 P || RCO°) Be P || PCO’) 

2 R || RC180°) BK R || PC180°) 8 P || R(180°) Be P || P(180°) 
3 R1R(90°) BR R| P(90°) 9 P_LR(90°) a P| P(90°) 
4 R|R(—90°)  R__P(—90°) 10 P_LR(—90°) a P| P(—90°) 
5 RER(90°) # REF P(90°) 11 PHFR(90”) 或 PFHPC90°) 

6 RER(—90°) a RFP(—90°) 12 PER(—90°) sk PFP(— 90°) 
例 5-17 基于 矢量 加 法 的 D-H 坐标 变换 。 

矢量 加 法 可 以 描述 从 一 个 坐标 系 变换 


























到 另 一 个 坐标 系 的 D-H 变换 。 如 图 5-17 
所 示 ， 在 坐标 系 B 中 一 点 P 的 坐标 可 以 
由 矢量 方向 确定 。 

O1P=O2P+0i02 (5-44) 





式 中 ， 
BO1O2 =(51 s2 s3)" (5-45) 
BO,P=(21 yı z1)! (5-46) 
2O,P=(x2 yz 22)? (5-47) 
然而 ， 利 用 两 个 坐标 系 坐 标 轴 夹 角 的 
余 纺 ， 可 以 将 矢量 的 坐标 表述 在 同一 坐标 
系 中 。 











图 5-17 导出 D-H 坐标 变换 的 另 一 种 方法 














wn 


L1=x2008(42 5x1) Fye2cosCy2 5x1) +z2cos(z2.xr1) +s 





yı =2%2C08(42591) Hy2cos(y2sy1) Fe2c08(z25y1) +52 














(5-48) 
zı =22c08s(x2 +21) +y2cosCy2 +21) +z2c08(z2 5.21) +53 
1l=22(0) | y2 (0) | 之 2(0) | 1 
调整 变换 式 〈5-48) ， 它 可 以 用 如 下 齐 次 矩阵 变换 描述 : 
XI cos(r25%1) cos(ye5x1) cos(Cz2 7Z1) $1)\(x2 
cos(xesy1) cos(y2.y1) cos(z2.y1) s 
yı = XL29¥1 2，yI1 2，y1 52 y2 (5-49) 
之 1 cos(xz2,z1) cos(y2,zl1) CoOs(z2,21) 3 || zs? 
1 0 0 0 1 1 

















在 图 5-17 中 ， 选 择 坐 标 轴 zx， ， 以 使 它 位 于 沿 着 坐标 轴 z 和 zs 的 最 短 的 公 垂 线 。 其 他 
的 参数 定义 如 下 : 

1) a 是 坐标 轴 ze 和 坐标 轴 >，*， 之 间 的 距离 。 

2) a 是 沿 着 a 将 坐标 轴 >i1 螺旋 旋转 至 坐标 轴 =， 的 旋转 角 。 
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3) d 是 从 坐标 轴 zi 到 坐标 轴 zs 的 距离 。 
4) 9 是 将 坐标 轴 zi 螺旋 旋转 至 坐标 轴 x 的 旋转 角 。 
AMF EGR EM FF UR AR TR HE BA BY AB 





Xl cosO —sinécosa 一 Sin0sina acosO\ /x2 
APT | sind coca cos@sina asinð || y2 E 
之 1 0 — sina cosa d 之 2 
1 0 0 0 1 1 
或 者 
‘pp =!T?°rp (5-51) 
式 中 ， 
1T,=(a,a,d ,0) (5-52) 


相对 于 坐标 系 Bl ， 参 数 a、a、d 、9 定义 了 坐标 系 B* ， 并 且 属 于 坐标 系 Bo. A, 0 

To Sai, ai. di, 0; 相对 于 坐标 系 B;- 1 定义 坐标 系 B 的 配置 ， 并 且 属 于 坐标 系 B, 。 
~1T; =(a;,a;5d;,0;) (5-53) 

例 5-18 同一 D-H 坐标 变换 矩阵 。 

因为 在 设置 坐标 系 的 D-H 法 中 ,， 平 动 距离 D 和 旋转 角 尺 是 沿 着 和 绕 着 一 个 坐标 轴 ， 是 
否 先 完成 平 动 距离 D 然后 再 旋转 形成 旋转 角 尺 反之 亦 然 ， 这 并 不 重要 。 因 此 ， 可 以 改变 沿 
着 和 绕 着 同一 坐标 轴 的 D 和 R 的 次 序 ， 并 且 可 以 获得 同一 D-H 变换 矩阵 [ 式 (5-11)]。 因 
此 ， 有 
































odes (5-54) 

克 例 5-19 D-H 法 应 于 平面 曲柄 块 连 杆 机 构 。 

对 一 闭环 机 器 人 或 者 机 械 机 构 ， 在 第 1 根 连 杆 和 最 后 一 根 连 杆 之 间 也 有 一 个 连接 。 因 
此 ， 这 种 连接 并 不 满足 D-H 法 的 定义 。 图 5-18 所 示 为 一 个 平面 曲柄 滑 块 连 杆 机 构 RL PHFR 
IRI R, 并且 在 每 根 连 杆 上 建立 D-H 坐标 系 ， 其 D-H 参数 见 表 5-7. 

表 5-7 图 5-18 所 示 的 平面 曲柄 滑 块 连 杆 机 构 的 D-H 参数 
































坐标 系 编号 ai ai di 0; 
1 az — 90° d 180° 
2 a3 0 0 03 
3 a4 0 0 04 
4 0 —90° 0 a; 











应 用 于 闭环 变换 ， 则 有 

we = (5-55) 

变换 矩阵 全 包含 所 有 含有 a2、d、a3、03、a4、04、01 参数 的 函数 。 其 中 参数 ar, 

az, a 是 常量 ， 而 参数 4、03、04 、01 是 变量 。 假 设 901 是 输入 变量 并 且 确 定 ， 可 以 通过 列 
写 T 和 工 中 相应 元 素 的 方程 ， 求 解 其 他 未 知 变量 93 和 04。 
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5-18 平面 曲柄 滑 块 连 杆 机 构 











k 5-20 非 标准 的 D-H 标记 。 

在 本 节 所 陈述 的 D-H 标记 是 标准 的 D- 贡 法 。 然 而， 我 们 可 以 采用 一 组 不 同 的 D-H 参 
数 ， 如 图 5-19 Prax, CEPT i 处 设置 连 杆 坐标 系 B; ， 而 不 是 在 末梢 关节 i 十 1 处 。z; 轴 
是 沿 着 关节 :的 轴线 ，z; 轴 是 =; 轴 和 >i+1 轴 的 公法 线 ， 方 向 从 =; 轴 到 zi;+1 轴 。y; 轴 是 坐 
标 系 B: 右手 法 则 所 确定 的 方向 。 

坐标 系 这 个 变换 的 参数 如 下 : 

1) a; 是 沿 着 x; 轴 且 位 于 zi; 轴 和 >;+1 轴 之 间 的 距离 。 

2) a; GAA x; WA z; 轴 到 >;+1 轴 的 角度 。 

D di 是 沿 着 x; 轴 且 位 于 x; 轴 和 zi+l 轴 之 间 的 距离 。 


























l 
1 
i 
1 
i 
L 
1 
1 
als 
ED | 
l 
1 
1 
\ a x 
il 


图 5-19 ”对 于 关节 和 连 杆 (i) 所 定义 的 D-H 参数 a;、a;、d;、0; 的 非 标准 定义 
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4) 0; 是 绕 着 x; 轴 从 x;-1 轴 到 x; 轴 的 角度 。 

利用 非 标 D-H 法 从 坐标 系 B;-1 到 坐标 系 B; 的 变换 矩阵 :本 ;-1 可 以 由 绕 着 和 沿 着 坐标 系 
B;-1 局 部 坐标 轴 的 两 次 旋转 和 两 次 平 动 获得 。 第 1 次 绕 着 x;-1 轴 旋转 a;-1， 第 2 KIRA 
Zz;--1 轴 平移 a;-1， 第 3 次 是 沿 着 z;-1 轴 平移 4;， 第 4 次 绕 着 z;-1 轴 旋转 0;。 




















'T;-1 =R aP 2;-1,—4;D acid i Ra mii 
COSO ; sinO;cosa;_1 sinĝ;sina;—ı —aj;—1cos0; 
一 Sin0; cosĝ;cosa;—ı cosĝ;sina;—ı ai— sing; 
= (5-56) 
0 — sina ;—| COSQ i—1 一 di; 
0 0 0 1 
因此 ， 利 用 非 标准 D-H 法 从 坐标 系 B 到 坐标 系 Bii AYER RET; 为 
cos; — sind; 0 Qi 一 1 
sing; cosa;—1 cosĝ;cosa;—1 —sina;—1; —d;sina;—| 
iT 一 | l | (5-57) 
sind; sina;—1  cos@;sina;—1 COS@;—1 d;cosa;—1 
0 0 0 1 





非 标准 D-H 法 的 一 个 优点 是 : 旋转 角 0; 是 绕 着 >; 轴 的 ， 关 节 的 数目 与 坐标 系 的 数目 
相同 。 施 加 在 关节 i 处 的 驱动 力 与 坐标 系 B: 也 在 同一 人 处。 选择 连 杆 的 几何 特性 ， 如 重心 ， 
位 于 该 系统 中 则 是 更 加 自然 的 事 。 

非 标准 D-H 法 的 缺点 是 变换 矩阵 是 指标 ;一 1 和 : 的 一 个 混合 。 

应 用 标准 或 非 标准 D-H 法 标记 是 一 个 个 人 喜好 问题 ， 因 为 这 两 种 方法 均 能 有 效 地 被 
应 用 。 








5.3 机 器 人 正和 疝 位 置 运动 学 











正 同 或 者 直接 运动 学 是 旋转 运动 中 从 机 器 人 关节 变量 空间 到 第 卡 儿 坐 标 系 空间 的 运动 变 
换 。 对 于 一 组 给 定 的 关节 变量 , 求 取 末端 执行 器 的 位 置 和 方向 是 正 向 运动 学 的 主要 问题 。 这 
个 问题 可 以 通过 求 取 用 于 描述 基体 连 杆 坐标 系 中 连 杆 (i) 运动 信息 的 变换 矩阵 "T; 而 得 到 
解决 。 对 于 机 械 手 ， 列 写 正 向 运动 学 方程 的 传统 方法 是 通过 D-H 标记 和 坐标 系 处 理 连 杆 而 
获得 的 。 因 此 ， 正 向 运动 学 是 基本 的 变换 运算 。 

对 于 一 个 6 自由 度 的 机 器 人 来 说 ， 有 6 个 D-H 变换 矩阵 ， 每 根 连 杆 一 个 D-H 28 He 
阵 ， 这 就 要 求 将 最 终 的 坐标 系 必须 变换 到 基体 坐标 系 之 中 。 附 于 最 后 坐标 系 的 最 终 坐 标 系 通 
常设 置 在 手 爪 的 中 心 处 ， 如 图 5-20 所 示 。 对 于 给 定 的 一 组 关节 变量 ， 可 以 唯一 地 确定 变换 
和 矩阵 一 人。 因此 ， 末 端 执行 器 的 位 置 和 方向 也 是 关 闻 变量 的 唯一 函数 。 

运动 学 信息 包括 位 置 、 速 度 、 加 速度 和 突变 。 然 而 ， 正 向 运动 学 通常 指 的 是 位 置 分 析 ， 
因此 正 向 位 置 运动 学 等 效 于 确定 一 个 综合 变换 和 矩阵"T, 。 

Ta =° Ti (q1)! T2 Cq2)? Tega) Taga 1T, (qn) (5-58) 


当 坐 标 在 最 终 坐 标 系 中 给 定时 ， 通 过 综合 变换 矩阵 "T， 可 以 求 得 点 尸 在 基体 坐标 系 中 
的 坐标 。 





































































































"rp =°T,” rp (5-59) 
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图 5-20 ”最 终 坐 标 系 在 基体 坐标 系 中 位 置 


例 5-21 3R 平面 机 械 手 正 向 运动 学 。 




















Al 5-21 一 个 RRIR 平 面 机 械 手 


5-21 所 示 为 一 个 RRR 平面 机 械 手 。 利 用 例 5-2 中 所 说 明 的 D-H 参数 ， 应 用 式 
(5-11) 我们 可 以 求 得 变换 和 矩 阵 !1T;， 其 中 i 二 3，2，1。 也 可 能 利用 例 5-9 中 的 变换 矩阵 
[st (5-32)] AXI G) 和 连 杆 (2) 都 平行 于 RC0"”)， 即 RR(0°)。 








cos03 一 Sin03 0 13cosO3 
Sin03 COSO3 0 /3Sin03 
?T; = (5-60) 
0 0 1 0 
0 0 0 1 
cos0» 一 Sin0， 0 lzcos02 
sind» cosd» 0 /7,sin0> 
IT, = (5-61) 
0 0 1 0 
0 0 0 1 
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25S 正 向 运动 学 
cosO1 —sind; 0 LicosOl 
sinfı cos 0 isini 
oT, = (5-62) 
0 0 1 0 
0 0 0 1 
因此 ， 将 末端 执行 器 变换 至 基体 坐标 系 中 的 变换 和 矩阵 为 
cos(0ı +02 +03) sin(@; +02 +03) O ru 
ors =° T1! T2? T; = sin(01 02 03) cos(CO1 +02 十 03 ) 0 r24 (5-63) 
0 0 1 0 
0 0 0 1 
ri4 二 lL1cos01 十 l2cos(01 十 90) 十 13cos(01 十 05 十 03) 
r2} =l sinfı +l2sin(0; +02) +l3sin(@; +02 十 03 ) 
坐标 系 Bs 原点 的 位 置 ， 即 机 器 人 的 末梢 点 位 置 矢量 为 
0 lı cos0; l2 cos(01 +62) +13cos(@, +62 +43 ) 
0 Z,sinO; +l2sin(@; +82) +23 sin(O; +42 十 03 ) 
Ts 一 (5-64) 
0 
1 1 





这 意味 着 如 果 已 知 机 器 人 的 几何 参数 和 所 有 关节 变量 ,我 们 可 以 求 得 末梢 点 在 基体 笛 卡 


儿 和 坐标 系 中 的 坐标 即 : 





X=licos01 十 12cos(01 十 02) 十 13cos(01 十 902 十 03) 
Y 王 /1Sin01 十 /Sin(O1 十 02 ) 十 /3 sin(ĝı +02 +63) 








机 械 手 的 平衡 位 置 位 于 co 坐标 轴 上 ，01 





1 0 
0 1 
oT, = 
0 0 
0 0 
所 求 得 的 变换 和 矩阵 和 1T; 能 够 完全 决定 





在 基体 坐标 系 中 的 变换 矩阵 为 





0, 02=0, 63 





0 
0 
1 
0 


Li Fla ls 
0 
0 
1 








oT, =° T1! T2 = f 
0 

fiu=licosh, ar 

fz =}; sin: T 


连 杆 (3) 在 坐标 系 B; 中 的 变换 矩阵 为 








cos(0ı +02) sin(0ı +02) 
sin(0ı +02) cos(0ı +02) 


0 

0 
l2cos(0; +02) 
Z2sin(O1 +02) 








IT; 一 1722 了 3 一 0 


0 





cos(02 +03) sin(02 +03) 
sin (02 +03) cos(02 +03) 


0 
0 


0 
0 
1 
0 


fu 

fa 
0 
1 


其 他 连 杆 坐标 系 中 每 根 连 杆 的 配置 





0， 因 此 "Ts 为 








(5-65) 
(5-66) 


(5-67) 


o EFF (2) 


(5-68) 


(5-69) 


179 





和 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





g14 王 /2cos0 ls3cos(0, +43) 





g4 =Llzsinfz 二 lssin(02 十 03 ) 
例 5-22 RFR || R 关节 臂 正 向 运动 学 。 
考虑 一 个 RFR | R 关节 臂 ， 其 原理 性 示意 如 图 5-22 所 示 。 为 了 研讨 机 械 人 正 向 运动 
学 ， 机 器 人 在 静止 位 置 处 的 D-H 参数 见 表 5-8。 机 器 人 静止 位 置 在 关节 轴 z1, z2, z 
处 可 以 进行 设置 ， 然 而 两 个 位 置 是 最 常用 的 。 第 1 个 位 置 就 是 zl 与 z1，z2，z3 共 面 , 第 2 


个 位 置 就 是 zu 与 z1，z2?，z3 共 面 。 

















图 5-22 RERI R ži 


R 5-8 建立 连 杆 坐标 系 的 D-H 参数 











坐标 系 编号 4i ai di 0; 
1 0 —90° dı 01 
2 lz 0 də 02 
3 0 90° ls 03 














我 们 推荐 应 用 从 例 5-9 到 例 5-14 中 所 给 出 的 连 杆 -关节 分 类 。 因 此 ， 我 们 必须 能 够 确定 


连 杆 -关节 合成 之 后 的 类 型 ， 见 表 5-9。 
表 5-9 建立 连 杆 坐标 系 的 连 杆 分 类 


























杆 号 关节 类 型 杆 号 关节 类 型 
1 RFR(—90°) 3 RER(90°) 
2 R || Ro’) 
因此 ， 连 续 的 变换 矩阵 为 
cos, 0 —sing; 0 
sind, 0 cos 0 
07T 1 = (5-70) 
0 = 0 dı 
0 0 0 1 
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cos, 一 Sin0， 0 le2cosd2 
sin® 2 cos 2 0 l2sind2 
IT, = (5-71) 
0 0 1 dz 
0 0 0 1 
cos; 0 sind 3 0 
， sin03 0 一 cos03 0 
“T's = (5-72) 
0 1 1 0 
0 0 0 1 
为 了 表述 最 终 的 变换 "Ts 
T; =° Ti! T2? T; (5-73) 
只 需求 得 矩阵 乘积 的 结果 ， 因 此 有 








oT, =° T1! T2? Ts = a (5-74) 


这 里 
ri1 =cos0; cos(@2 +63) (5-75) 
r21 =sinO; cos(02 +43) (5-76) 
r31 =—sin(@2 +43) (5-77) 
rı: =— sing) (5-78) 
r22 一 COSO1 (5-79) 
r32 =0 (5-80) 
r13 =cos0; sin(O2 +43) (5-81) 
r23 =sind; sin (02 +43 ) (5-82) 
r33 =cos(O2 十 03 ) (5-83) 
ri4 =l2cos0) cos02 —d2 sind, (5-84) 
r24 =L2cos02 sinfı +d2cos@, (5-85) 
r34 =d; —l2sinf2 (5-86) 


第 3 个 机 械 臂 的 末梢 点 P 在 坐标 系 Bs 中 的 位 置 撩 量 为 (0 0 Z), K, CEER 
坐标 系 中 的 位 置 矢量 为 








0 —d2sin0; 十 /2cosOl cosd2 +13 cosd; sin(O2 十 03 ) 
0 d»cos0, 十 /2cosoy sin01 十 13Sin0l sin(0, +03) 
rp 一 "Ts3rp 一 0T3 一 (5-87) 
L3 dl1 一 /2Ssin0 十 /3cos(C0 十 03 ) 
1 1 
当 01 二 0、0s 二 0、0s 二 0 时 ,静止 位 置 的 变换 矩阵 为 
1 0 0 2 
0 1 0 de 
0T 3 = (5-88) 
0 0 1 di 
0 0 0 1 
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这 组 坐标 变换 矩阵 说 明 ， 在 静止 位 置 ，x1、x，、x3 IR] 





线 ， 之 1]、 之 2 平行 。 
例 5-23 ”关节 臂 运动 学 。 








考虑 图 5-22 PH RER | R 关节 辟 ， 并 日 具有 如 下 尺寸 : 
=0.48m.d2=0. 174m. 





l2=0.75m,l3=0. 65m,d 
利用 表 5-9 中 的 连 杆 -关节 合成 ， 有 














coOSO1 0 一 Sin01 0 
sing | 0 coOSO1 0 
oT 一 
0 = 0 0.48 
0 0 0 1 
cosO， 一 Sin0， 0 0. 75cos@2 
sinf2 cos 2 0 0. 75sin@2 
IT, — 
0 0 1 . 174 
0 0 0 1 
cosO3 0 sins 0 
sind; 0 —cosé3 0 
2 了 3 — 
0 1 0 0 
0 0 1 


因此 ， 从 坐标 系 Bs 到 坐标 系 Bo 的 变换 矩阵 为 


coSOl cos(@2 十 03 ) 


sin0l cos(@2 +03) 
T; =° T1! T2? T; = ; 
一 Sin(O +03) 


0 





一 Sin01 cos01sin(0, 十 03 ) 





coSsO1 sin0l sin(@2 +43) 
0 cos(G2 +03) 
0 0 


rıı =0. 75cos@1 cos@z —0.174sin01 
ri2=0. 174cos@, +0. 75cos@2 sind, 
r13 =0. 48—0. 75sin@2 

第 3 根 连 杆 的 末梢 点 〈 端 点 ) P 的 位 置 矢量 为 


0 


rp 一 0T33rp ="T,= = 


rı 

r2 

0. 65 r3 
T 1 


rı =0. 75cos0; cosd2 一 0. 174sind; +0. 65cosdi sin (82 十 03 ) 
rz =0. 174cos01 +0. 75cos@2 sind; +0. 65sin0l sin(02 +03 ) 
r3 =0. 65cos(@2 +43) —0. 75sinfz +0. 48 

TE 01 =0, 02=0, 03=0 的 静止 位 置 ， 变 换 矩 阵 为 











1 0 
0 1 

07 3 一 

0 0 

0 0 
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0 
0 
1 
0 








0. 75 

0. 174 

0. 48 
1 





昌平 行 于 xo， 而 且 zi, z3 dt 


(5-89) 


(5-90) 


(5-91) 


(5-92) 


(5-93) 


(5-94) 


(5-95) 





sk (5-95) 表明 ， 静 止 位 置 的 末梢 点 〈 端 点 ) 的 位 置 矢量 为 


0 0.75 

0 0.174 
On, =9T 23 p, =—°T, = = (5-96) 
ee So es) |1.18 

1 1 


例 5-24 机 械 手 的 工作 空间 。 
考虑 图 5-22 中 的 RER|| RXTE, HRA WFR: 
1, =0. 75m.l3 =0. 65msd; =0. 48m.d2=0. 174m (5-97) 
连 杆 变换 矩阵 在 式 (5-90) ~} (5-92) 中 已 经 给 出 。 当 点 P 位 于 式 (5-96) 处 时 ， 机 
械 手 在 静止 位 置 的 变换 矩阵 "Ts 是 式 (5-95)。 
假设 每 个 关节 点 都 能 旋转 360"。 理 论 上 ， 点 已 必须 能 够 到 达 球 域 S1 中 的 任何 点 ， 以 及 
ERER S 以 外 的 点 。 即 





Cr —°d 1—d2°R1)?= (213)? (5-98) 
x?+(y—0. 174)2 十 (> 一 0. 48)2 一 1. 96 (5-99) 
0 0 
od 一 | 0 |=| 0 (5-100) 
di 0. 48 
1 0 0 0 0 
dsok1=°Ridsikhi=|I0 0 1 0 |=|0.174 (5-101) 
o 一 1 0 八 0.174 0 
(r —"d ı—d2°k1)? =(l2— l3)? (5-102) 
x? +(y—0. 174)? +(z—0. 48)? =0. 01 (5-103) 


球 域 Si 和 球 域 So 之 间 的 可 到 达 空 间 称 为 机 械 手 的 工作 空间 。 
例 5-25 SCARA 机 械 手 正 向 运动 学 。 
考虑 如 图 5-23 所 示 的 R || RI RP 机械手 。 机 械 手 正 向 运动 学 可 通过 求 取 每 个 变换 和 矩 








图 5-23 RII RI RI P HLI 








人 学 : 


WE wens, 


运动 学 、 动 力学 与 控制 技术 











HIE FFE R || R(0") 连 


cos 


o Hl 
一 Sin01 
cos 
0 
0 


sing 
0 
0 


oT 





杆 ， 其 变换 矩阵 为 


0 
0 Lisinĝi 
1 0 
0 1 


lı cos] 


第 2 根 连 杆 也 是 R || RCO?) 连 杆 ， 其 变换 矩阵 为 


四 
又 





cos0» —sinds 
cosd 2 
0 


0 


sind 2 
0 
0 





IT; = 





具有 零 长 度 的 RI RO?) 连 杆 ， 其 


COSO3 
cos? 


0 


sind 3 
3 一 
0 
0 


2 





Oo = = 


0 
`T, 一 0 
1 
0 


oO O O Fe 


0 


一 Sin03 


l2COSO2 
l2 sinĝ2 
0 
1 


变换 矩阵 为 


0 
0 
1 
0 





0 0 
3 0 0 
1 0 
0 1 


后 ,第 4 根 连 杆 是 一 个 RlP(0") 连 杆 ， 其 变换 矩阵 为 


0 
0 
d 
1 


HIE, FEE PAS AIS fos ARP, A Ii DEA ie BY BS HBO 


T, =°T 7 'T2?T3°T, 





























cos(0ı +02 +03) sin(0ı +0: +03) 0 
sin(0ı H02 +03) cos(0ı +02 +03) 0 
7 0 0 1 
0 0 0 
式 (5-108) KHH, SLIPS HILAL (01 = 02 
1 0 0 
oT, 一 0 1 0 
0 0 1 
0 0 0 


例 5-26 


空间 站 机 械 手 遥控 系统 (SSRMS). 


十 /zcos(O1 +02) 


Lı cos 





Li sinfı 十 /2 sin(CO1 +02) 
d 
I 
=0; =d =0) 的 变换 矩阵 为 
li +l, 
0 
0 
1 


(5-104) 


(5-105) 


(5-106) 


(5-107) 


(5-108) 


(5-109) 


空间 站 机 械 手 遥控 系统 (Space Station Remote Manipulator System, SSRMS) 也 被 称 


为 航天 飞机 遥控 机 械 手 系统 (Shuttle Remote Manipulator System, SRMS), 
< 间 站 上 的 一 
间 站 、 在 机 械 臂 的 末 

图 5-24 所 示 为 SRMS 简化 原理 模型 ， 





天 飞机 或 者 空 
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LU vo 


Yi IB 


去 送 航天 员 、 





mr 


已 





间 站 外 面 的 事物 等 。 





是 依附 在 航 


个 机 械 手 臂 和 机 械 手 。 使 用 该 系统 有 几 个 目的 卫星 调度 、 构 建 空 
使 用 照相 机 拍摄 和 检查 空 
它 的 特性 参数 见 表 5-10, 








图 5-24 空间 站 遥控 机 械 手 系统 〈 不 成 比例 ) 









































表 5-10 空间 站 机 械 手 的 特性 参数 

长 度 /m 14. 22 
直径 /cm 38.1 
质量 /kg 1336 

关节 数 7 

处 理 能 力 /kg 116000( 在 太空 中 ) 
空 载 37 
末端 最 大 速度 / (cm/s) 

满载 1.2 

最 大 旋转 速度 /(C/s) 4 











考虑 关节 偏 置 值 和 连 杆 长 度 值 ， 见 表 5-11。 利 用 这 些 值 说 明 每 根 连 杆 的 类 型 ， 从 而 使 
[ii] 1B 
































我 们 能 够 确定 求解 正 癌 运动 学 问题 所 要 求 的 变换 和 矩阵。 
表 5-11 SRMS 的 D-H 参数 
坐标 系 编号 a a d 0 
1 0 —90 380mm 01 
2 0 一 90 1360mm 0 
3 7110mm 0 570mm 03 
4 7110mm 0 475mm 04 
5 0 90 570mm 0 
6 0 一 90 635mm Os 
7 0 0 d7 0 














EFF (1) 和 连 杆 (2) 是 RHFR( 一 90")， 因 此 有 
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coOSO1 0 一 Sin01 0 
sing | 0 cos 0 
oT] = (5-110) 
0 =] 0 dı 
0 0 0 1 
cosy 0 一 Sin0， 0 
sinf2 0 cos0» 0 
IT, = (5-111) 
0 = 0 d2 
0 0 0 di 
连 杆 (3) 和 连 杆 (4) 是 Rl RO), 因此 有 
cos03 一 Sin03 0 a3cos0s 
sind 3 cos03 0 assins 
T; = (5-112) 
0 0 下 d3 
0 0 0 1 
coSsO4 一 Sin04 0 adcosO4 
sind 4 COSO14 0 adsin04 
3T 4 一 (5-113) 
0 0 1 di 
0 0 0 1 
连 杆 (5) 是 REFR(C90"),， 连 杆 (6) 是 REFR( 一 90")， 因 此 有 
cos; 0 sind 5 0 
sinds 0  —cosés 0 
4T 5 = (5-114) 
0 1 0 ds 
0 0 0 1 
coss 0 一 Sin06 0 
- sins 0 costs 0 
ST; = (5-115) 
0 一 小 0 ds 
0 0 0 1 
连 杆 (7) 是 RFR(0")， 附 着 在 末端 执行 器 上 的 坐标 系 相 对 于 坐标 系 Be 有 一 个 平 
di. 
cos07 —siné; 0 0 
sin07 cos07 0 0 
6T; = (5-116) 
0 0 1 d7 
0 0 0 il 
AS HEMET; (i 二 1,2,…,7) 的 正 向 乘积 可 求 得 SSRMS 的 正 向 运动 。 
OT; =° T1! T2? T33 T44 T5” T6? T7 (5-117) 


5.4 FOES bi 


5-25 所 示 为 一 个 球形 关节 。 球 形 关节 连接 着 前 臂 和 机 械 手 这 两 个 连 杆 。 前 臂 和 机 械 
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手 连 杆 手 爪 坐标 系 
图 5-25 具有 翻滚 -俯仰 - 偏 航 旋转 运动 的 球形 关节 


手 的 旋转 轴 在 平衡 位 置 被 认为 是 共 线 的 。 机 械 手 的 旋转 轴 称 为 手 扑 轴 。 球 形 手腕 是 连 杆 和 关 
节 的 合成 ， 用 来 模拟 球形 关节 ， 并 且 为 手 爪 连 杆 提供 3 个 旋转 自由 度 。 它 由 具有 零 长 度 和 有 零 
偏 距 的 3 个 RER 连 杆 所 构成 。 在 这 3 个 连 杆 中 ， 它 们 的 关节 轴 是 相互 正 交 的 ， 并 且 相 交 于 
一 点 ， 该 点 称 为 手腕 点 。 手 腕 点 在 机 器 人 结构 中 是 不 变 的 ， 并 不 随 着 手腕 角 坐 标的 改变 而 
改变 。 

在 手腕 点 处 ， 我 们 定义 了 两 个 坐标 系 。 第 1 个 坐标 系 是 手腕 固定 坐标 系 ， 该 坐标 系 附 着 
在 前 臂 连 杆 上 ; 第 2 个 坐标 系 是 手腕 活动 坐标 系 ， 该 坐标 系 附着 在 手 扑 连 杆 上 。 我 们 也 引入 
一 个 工具 或 可 称 为 手 扑 坐标 系 。 手 腕 坐标 系 由 3 个 矢量 所 定义 ,a =k. s=, n=). TX 
手 的 手指 之 间 的 一 个 对 称 点 处 设置 手 爪 坐标 系 ， 或 者 在 手指 夹 持 工具 的 尖端 处 设置 手 扑 从 
标 系 。 

图 5-26 所 示 为 球形 手腕 的 结构 。 它 的 构成 : 一 个 RER(—90°) 连 杆 串 接 到 男 一 个 RER 
(90°) 连 杆 上 ， 最 后 再 连接 到 旋转 手 爪 连 杆 R || ROS 上 。 手 爪 坐标 系 Br 总 是 平行 于 坐标 
系 Bs ， 并 且 位 于 距 手 腕 点 距离 为 dz 处 。 







































































连接 到 前 臂 
图 5-26 球形 手腕 的 结构 


为 了 对 球形 手腕 进行 分 类 ， 我 们 将 球形 手腕 的 旋转 分 解 为 绕 着 3 个 正 交 轴 的 3 个 旋转 运 
动 ， 翻 滚 运 动 、 俯 仰 运 动 、 偏 航运 动 ， 如 图 5-25 所 示 。 翻 滚 是 当 手 腕 位 于 平衡 位 置 时 手 爪 
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绕 其 轴线 的 任何 旋转 运动 (有些 书籍 也 将 此 运动 称 为 手 转 )。 手 不 轴 4 定义 了 一 个 垂直 于 被 
称 为 手 爪 墙 轴 线 的 平面 + Xn 。 俯 佩 和 偏 航 是 在 手腕 点 处 的 手 扑 墙 内 绕 着 两 相互 垂直 轴 的 旋 
转运 动 。 在 手腕 的 静止 位 置 也 定义 了 翻滚 、 依 仰 、 偏 航运 动 。 

实际 的 球形 手腕 有 3 种 类 型 ， 其 分 类 见 表 5-12。 
表 5-12 球形 手腕 的 分 类 
































类 型 旋转 顺序 类 型 旋转 顺序 
翻滚 -俯仰 -翻滚 3 俯仰 - 偏 航 -翻滚 
2 翻滚 -俯仰 - 偏 航 














5-26 所 示 为 一 个 翻滚 -俯仰 -翻滚 球形 手 有 晓 ， 其 变换 矩阵 为 
8T 5 一 3T44T5 5T14 


cos, coss coss — sinf, sins 一 cosb6 Sin04 一 cosO4 cos0O5 Sin06 cos@,sinés 0 
| cosĝ4 sin06 十 cosb5 cos06 sin. coOSO4cos06 — cos@5 Sin04 Sing06 Sin04sin05 0 
7 — coss sind; sinds sind. COSO5 0 
0 0 0 1 

(5-118) 


证 明 : 

一 个 手腕 连接 至 机 械 手 的 最 后 一 个 连 杆 上 ， 通 常 是 连 杆 (3)。 机 械 手 最 后 一 个 连 杆 坐标 
系 应 该 建立 在 手腕 点 处 ， 以 便 充 当 手 腕 机 构 的 基体 连 杆 。 如 果 机 械 手 有 3 根 连 杆 ， 那 么 手腕 
机 构 的 第 1 个 坐标 系 应 该 标注 为 3， 并且 与 机 械 手 坐标 系 Bs 具有 相同 的 定向 。>zs 轴 是 沿 着 
第 1 个 手腕 关节 所 指 的 方向 。 当 手腕 是 如 网 5-26 所 示 的 结构 时 ，>s 轴 将 是 沿 着 机 械 手 前 臂 
的 轴线 。 我 们 选择 rs = 24 Xzs 和 w 王 一 90"， 因 为 当 05 =0 时 我 们 希望 机 械 手 来 自前 臂 。 

对 于 连 杆 (4) 利用 式 05-37), SPF (5) 利用 式 〈5-36)， 对 于 连 杆 6) 利用 






































R.z.05 9 对 于 坐标 系 Br 有 D z.d; 9 我 门 求 得 下 列 变换 矩阵 : 
COSO4 0 一 Sin04 0 
sind 4 0 COSO14 0 
3T; 一 (5-119) 
0 =] 0 0 
0 0 0 1 
coOSO5 0 sinds 0 
sinds 0 一 cos0O5 0 
1T; = (5-120) 
0 1 0 0 
0 0 0 1 
coss sin06 0 0 
- sinfe cosl s 0 0 
"T; = (5-121) 
0 0 1 0 
0 0 0 1 
1 0 0 0 
0 1 0 0 
6T7 = (5-122) 
0 0 1 dr 
0 0 0 下 
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HERET =8T ‘TT. 提供 了 手腕 在 前 臂 坐 标 系 Bs 中 的 定位 。 
376 一 3T44 了 55 了 6 








cosO4 coss cos06 一 Sin04Ssin06 一 cosO6 sin04 一 cosO4cos05 sin06 cosdysings 0 
coSsO4 sin06 十 cosgi coss Sin04 cosO4cosb6 — cosĝ; Sin04 sind. sin04sin05 0 
o — coss sinf; Sin05 sinĝ6 coOSO5 0 
0 0 0 1 

(5-123) 


下 面 的 变换 矩阵 提供 了 坐标 系 Bz 在 前 臂 坐标 系 Bs 中 的 定向 。 
3T; =3T !T5°T 5 Ty 
cosl, cosb5 cos06 一 Sin04sin06 ”一 cosO6 sin04 一 cosO4 cosO5 sinfe  cos0asinds d7cos0isinds 
cos, sin06 + cos cos sin04 cosO4 cosO6 一 cosO5 sinb 4 sins sind, sind; d7sinĝ, sins 
一 cosO6 sinds sins Sin06 COSO5 d7cos05 
0 0 0 1 
(5-124) 


tH AY DA ee SO fay 24 A ER OT. KPEE ES A 90。 和 平移 距离 d7. 
cos0e —sindgs 0 0 


sind coss 0 0 














STe = (5-125) 
0 0 1 dz 
0 0 0 下 
应 用 简约 的 变换 抢 阵 57T4 减少 了 矩阵 数目 和 数值 计算 的 数目 。 
在 平衡 位 置 处 的 变换 矩阵 (094 二 0、095s 二 0、06 二 0) 为 
1 0 0 0 
0 10 0 
°T; = (5-126) 
0 0 1 d7 
0 0 0 1 














为 了 说 明 仅 有 3 种 类 型 的 球形 手腕 ,我 们 从 手 脐 的 第 1 个 旋转 运动 开始 ， 该 旋转 运动 总 是 绕 
着 前 臂 连 杆 上 的 固定 轴 和 运动 。 在 平衡 位 置 ， 这 个 轴 也 是 沿 着 手 爪 轴 的 。 如 果 第 1 Se ee HH 
直 于 前 臂 轴 ， 则 可 以 考虑 将 第 1 个 旋转 运动 作为 俯仰 旋转 运动 。 对 于 第 3 个 旋转 运动 ， 有 两 
种 可 能 的 情况 : 如 果 是 绕 着 手 扑 轴 旋 转 ， 那 么 第 3 个 旋转 运动 就 是 翻滚 旋转 和 运动， 如果 垂直 
于 前 两 个 旋转 运动 ， 那 么 它 就 是 一 个 偏 航 ( 摆 ) 旋转 运动 。 

5-27 和 图 5-28 所 示 分 别 为 处 于 平衡 位 置 和 处 于 运动 中 的 一 个 翻 深 - 俯 佩 - 偏 航 手腕 。 
这 种 类 型 的 手腕 也 被 称 为 欧 拉手 腕 (Eulerian wrist)， 仅 因为 翻滚 -俯仰 - 偏 航 使 人 想起 了 
Z 一 x 一 z 旋转 轴 。 

如 果 第 1 个 旋转 运动 是 俯仰 运动 ， 则 第 2 旋转 运动 可 以 是 一 个 翻 深 运 动 或 者 偏 航 运动 。 
如 果 第 1 个 旋转 运动 是 偏 航运 动 ， 那 么 第 3 个 旋转 运动 肯定 是 一 个 独立 的 翻 深 运 动 。 如 果 第 
1 个 旋转 运动 是 翻滚 运动 ， 那 么 第 3 个 旋转 运动 肯定 是 一 个 偏 航运 动 。 俯 仰 - 偏 航 -翻滚 和 念 
仰 -翻滚 - 偏 航 是 没有 区 别 的 ， 因 此 我 们 可 以 将 俯仰 - 偏 航 -翻滚 作为 将 俯仰 运动 作为 第 1 个 旋 
转运 动 的 唯一 可 能 的 球形 手腕 。 

实际 上 ， 通 过 在 固定 坐标 系 和 活动 坐标 系 之 间 引 入 两 个 连 杆 和 3 个 坐标 系 ， 提 供 翻 滚 运 
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5-28 ”处 于 运动 中 的 机 械 手 


动 、 俯 仰 运动 和 偏 航 运动 。 连 杆 用 3 个 旋转 关节 相连 接 。 关 节 轴 在 手腕 点 处 相交 ， 并 且 当 手 
腕 处 于 平衡 位 置 时 关节 轴 是 正 交 的 。 

如 果 通 过 在 手腕 点 处 定义 3 个 非 D-H 坐标 系 从 运动 学 上 分 析 球 形 手腕 ， 如 果 定 义 它 们 
的 相对 变换 ， 那 么 手腕 的 旋转 运动 将 更 加 简单 。 图 5-29 所 示 为 一 个 具有 3 个 坐标 系 的 翻滚 - 
俯仰 -翻滚 〈 欧 拉 ) 手腕 。 第 1 个 正 交 坐标 系 B(x。，wo。，zo) 固定 在 前 臂 上 ， 起 着 手腕 固 
定 坐 标 系 的 作用 ，xz。 轴 是 前 臂 的 关节 轴 和 旋转 连 杆 。 旋 转 连 杆 是 第 1 个 手腕 连 杆 ， 关 节 是 
第 1 个 手腕 关节 。x。 Ai, yo 轴 的 方向 是 任意 的 。 定 义 第 2 个 坐标 系 B Cis yi zi) M 
而 使 =, 轴 在 平衡 位 置 能 够 沿 着 手 爪 轴 ，>zi 轴 是 第 2 个 关节 轴 。Bi (zx1，y1，z1) BHAA 
zo 轴 旋 转 角 ， 相 对 于 坐标 系 Bu (29. yos zo) BA x, 轴 旋 转 0 角 。 第 3 个 坐标 系 
B;(z*，y*，x?) 是 手腕 活动 坐标 系 ， 定 义 坐 标 系 B2, y2 z2), MAME zo 轴 总 是 沿 着 
FM. WER 3 个 关节 提供 一 个 翻滚 旋转 运动 ,那么 xz; 轴 就 是 关节 轴 ， 否 则 第 3 个 关节 
Memi GE) 旋转 和 运动，z，* 是 关节 轴 。 因 此 ， 相 对 于 坐标 系 B (x1，y1，x1) WMA, M 
标 系 B, (ro. vos 22) MERA z MRA x, WIER y 角 。 引 入 坐标 系 Bi Cis yis zi) 
和 Bs,(x，。，y，，zs) 是 为 了 简化 球形 手腕 运动 学 ,不 用 看 手腕 内 部 的 连 杆 。Bi Ci yis 
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z1) 相对 于 坐标 系 Bu(Czu，yo，zo)， 考 虑 坐标 系 B, (z* yo. z2) 相对 于 坐标 系 B (zl1， 
y1，z1) 的 定义 和 旋转 ， 实 际 中 的 球形 手腕 仅仅 有 3 种 类 型 ， 其 分 类 见 表 5-11。 这 3 种 手 
腕 见 图 5-29 一 图 5-31, 

例 5-27 D-H 坐标 系 和 球形 手腕 。 

5-27 和 图 5-28 所 示 为 类 型 1 球形 手腕 的 另 一 种 说 明 。 坐 标 系 Bis Bs. Bo 的 原点 位 
于 手腕 点 处 。 最 后 的 坐标 系 ， 称 为 末端 执行 器 坐标 系 ， 它 由 a 、s 、n 3 个 矢量 所 标记 ， 建 
立 在 空手 手指 之 间 的 对 称 点 处 或 者 机 械 手 夹 持 工具 的 末梢 处 。 矢 量 被 称 为 俯仰 (倾斜 ” 矢 
量 ， 它 是 一 个 正 交 矢量 ， 垂 直 于 手指 或 者 偏 航 〈 摆 )。 矢 量 s 被 称 为 扭转 矢量 ， 它 是 一 个 滑 
动 矢 量 ， 表 明 手 指 张 开 的 方向 。 矢 量 a 被 称 为 旋转 矢量 ， 它 是 接近 矢量 ， 垂 直 于 机 械 手 的 
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内 部 连 杆 坐 标 系 的 位 置 可 由 D-H 法 预先 确定 ， 然 而 对 于 末端 连 杆 ， 坐 标 系 B, 的 位 置 不 
知 什么 原因 ， 它 是 任意 的 。 通 过 简化 选择 或 者 通过 手 爪 中 的 特定 的 位 置 ， 可 以 重新 求解 任意 
性 。 如 果 使 >， 与 ,1 一致 ， 用 坐标 系 B, 分 析 将 更 容易 一 些 。 这 个 选择 使 a, 一 0 并 且 
os 

例 5-28 翻滚 -俯仰 - 俩 航 或 者 欧 拉 手腕 。 

5-29 所 示 为 类 型 1 球形 手腕 的 翻 深 - 俯 仰 - 偏 航 。 坐 标 系 Bo Crys yos zo) 是 固定 坐标 
系 ， 坐 标 系 B(x; y2» z2) 说 明了 它 的 活动 坐标 系 。 变 换 和 矩阵 "Ri ERE zo MIER o 
角 ， 接 着 绕 zi 轴 旋 转 0 的 变换 矩阵 。 









































图 5-29 ” 翻 深 - 俯 仰 - 偏 航 ( 欧 拉 ) 手腕 


OR, 一 Rj 一 CR a oR] ]T= [R20RZ,, 


Z09@ 


1 0 0 \(cosp 一 Sinp 0)\T)T 
=|10 cos? sinô || sing cosp 0 
0 一 Sin0 cos 0 0 1 


cose —cosdsing — sin@sing 
(5-127) 


=|sing cosdcose —cosgsiné 





0 sind cos? 





变换 和 矩阵!R2， 是 绕 着 局 部 轴 z2 旋转 y 角 的 。 
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cosy 一 Sinw 0 
1Rs=?RI=R!,,y—=Ri,y=|siny cosy 0 (5-128) 
0 0 1 
因此 ， 活 动手 腕 坐标 系 和 固定 手 胱 坐标 系 之 间 的 变换 矩阵 为 
OR? =°R R; S[R a, Rig TRE y =R z0. R T RT, yR z, RIRI y 











zoso 
cosWcosp 一 cosOSsinWsinp 一 cosWsinp 一 cosgcosWsinp — sindsing 
=| cosWsinp 十 cosOcospsinm 一 SinWsinp 十 cosgcosWcosp —cosgsind 
sind sind cosysinO cosé 
(5-129) 





例 5-29 翻滚 -俯仰 - 俩 航 球形 手腕 。 
图 5-30 所 示 为 一 个 类 型 2 AREF, R-N- RR Boo, yos zo) EF 
腕 固定 坐标 系 。 这 种 类 型 球形 手腕 的 主要 运动 学 缺点 是 : zl 轴 并 不 是 固定 在 手 扑 上 。 然 而 
我 们 将 坐标 系 Bore, yor z2) 附加 于 手 爪 ， 作 为 手腕 活动 坐标 系 ， 以 使 zs 轴 位 于 手 爪 轴 
E, x: 轴 是 第 3 个 关节 轴 。 坐 标 系 Bo 和 Bi 之 间 的 变换 紧 紧 围绕 着 x 轴 旋 转 y 角 。 














图 5-30 翻滚 -俯仰 - 俩 航 球形 手腕 


1 0 0 NT 
IRs=Ri,,y=Ri,y=|0 cos sing (5-130) 
0 一 SinW cosy 
为 了 确定 变换 矩阵 "R1， 我 们 将 坐标 系 Bi 首先 绕 着 zo 轴 旋 转 yp 角 ， 然 后 绕 着 x1 轴 旋 
kofa. KI, € 














OR, = R= (R,, oR! ) 一 (RroR7Z.¢)" 





zoso 
1 0 0 cosp 一 Sinp 0)\T\T 
二 ||0 cos0O sin@ || sing cosp 0 
0 一 Sin0 cosO 0 0 1 
cosp cosOsing — sinésing 
=|sing costcosp —cososinð (5-131) 
0 sind cos 





因此 ， 活 动手 胸 坐 标 系 和 固定 手腕 坐标 系 之 间 的 变换 矩阵 为 
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°R2=°R,'R2= [Rx RI, p TRI, =Re.gRr oR is.y=Rz.R oR ty 
cosg  sindsin#sing — cos cossing cosdsinwsing + cos#sind sing 


二 | Sinp cosdcos#cosy—cosgsindsing —cosdcoscosg — coswsindcosg | (5-132) 


0 cos sing + cos sind cosOcosy — sind sin 
例 5-30 RF AM Ain R ERIE F i 
5-31 所 示 为 一 个 类 型 3 AY BRIG FF Ib. B AAG fc - SF be. AR bn AA Bo lros yos 
zo) 是 手腕 固定 坐标 系 ， 坐 标 系 Bora, yo. z2) 是 活动 坐标 系 。 变 换 和 矩阵 IR， 是 绕 着 局 
部 zs 轴 旋 转 y 角 。 



































图 5-31 ”俯仰 - 偏 航 -翻滚 球形 手腕 





cosy —sing 0 
1Rs=?RI=R!,,y—=Ri,y=|siny cosy 0 








(5-133) 
0 0 1 

为 了 确定 变换 矩阵 "R1， 我 们 将 坐标 系 Bl (x1，y1，x1) SEA zo WIER pg 角 ， 然 后 再 

绕 着 x1 轴 旋 转 0 角 。 








oR, = R? = (Rz,,Riy,9)) = (Rx oRZ,g)T 





1 0 0 cosp —sing 0)\T\T 
二 ||0 cos@ sin@ || sing cose 0 
0 一 Sin0 cosé 0 0 1 
cose —cosésing — sinésing 
=|sing cosdcosp  —cosgsind (5-134) 
0 sind COSO 


因此 ， 活 动手 胱 坐标 系 和 固定 手腕 坐标 系 之 间 的 变换 矩阵 为 
OR: =°RI1R,= [Ra oR] JTRi1,,, =R, RI 


和 T pT 
zx0y0 Zog 1 OR zp SRZ, R oRey 
cosWcosp 一 cosOSsinWsinPp ”一 cospsinVW 一 cosgcosWsinp sin0sing 
二 | cosWsinp 十 cosOcospsinW 一 SinWsinp 十 cosgcosWcosp 一 Sin0cosp (5-135) 


sindsinw coswsind cosé 


例 5-31 球形 手腕 的 实际 设计 。 
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5-32 所 示 为 一 个 实际 的 欧 拉 球 形 手 舌 。 翻 滚 -俯仰 - 翻 深 的 3 个 旋转 由 3 个 共 轴 的 旋转 
轴 控 制 。 第 1 个 旋转 是 坐标 系 Bi SEA zs 轴 的 翻滚 运动 。 第 2 个 旋转 是 Bs 绕 着 zs 轴 的 俯仰 
运动 。 第 3 个 旋转 是 Bo SEA xs 轴 的 翻滚 运动 。 


















































| a a 
图 5-32 一 个 实际 的 欧 拉 球 形 手腕 


5.5 组 装运 动 学 


大 部 分 现代 工业 机 器 人 都 有 一 个 主要 的 机 械 手 和 一 系列 互 变 的 手腕 。 机 械 手 是 多 体 的 ， 
以 便 它 能 抓 住 主要 的 力学 单元 ， 并 且 为 手腕 提供 一 个 强 有 力 的 运动 。 

5-33 所 示 为 一 个 具有 3 自由 度 的 关节 机 械 手 。 这 个 机 械 手 可 以 通过 在 M1 基 座 电动 
机 相对 于 全 局 〈 绝 对 ) 坐标 系 旋 转 ， 并 且 在 Me 和 Ms 处 承载 其 他 电动 机 。 

可 变 手 腻 是 复杂 多 体 ， 主 要 用 来 提供 绕 着 腕 点 的 3 个 旋转 自由 度 。 手 腕 基本 连接 至 机 械 
手 的 末端 点 。 机 右 人 的 手 胸 、 实 际 操作 部 件 也 可 以 称 为 末端 执行 器 、 手 爪 、 手 。 图 5-34 所 
示 为 一 个 球形 手腕 ， 假 设 它 连接 到 图 5-33 中 的 机 械 手 上 。 


















































图 5-33 具有 3 自由 度 的 关节 机 械 手 
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为 了 求解 组 装机 器 人 的 运动 ， 我 们 将 机 械 手 和 手腕 作 当 作 独 特 的 复合 刚体 进行 考虑 。 然 
而 ,我们 在 机 械 手 端点 处 连接 一 个 临时 坐标 系 ， 在 手腕 基点 处 连接 男 一 个 临时 坐标 系 。 在 临 
时 端点 处 的 坐标 系 称 为 takht AB bp A. 手腕 基 座 处 的 坐标 系 称 为 neshin 坐标 系 ， 匹 配 takht 
坐标 系 和 neshin 坐标 系 在 运动 上 便 组 装 成 了 机 器 人 。 和 手腕 和 机 械 臂 的 运动 匹配 称 为 组 装 。 

5-33 中 的 坐标 系 Bs 是 机 械 手 的 takht RR, KI 5-34 中 的 坐标 系 Bo 是 手腕 的 
neshin 坐标 系 。 在 组 装 过 程 中 ， 手 腕 neshin 坐标 系 Bo 位 于 机 械 手 takht 坐标 系 Bs 之 上 ， 以 
便 xs 和 zo 一 致 ，zsg 和 ze 一 致 。 通 过 组 装 球形 手 腻 和 关节 机 械 手 所 形成 的 关节 机 器 人 如 图 
5-35 所 示 。 























Xg 连接 至 前 臂 
图 5-34 球形 手腕 及 其 运动 




















基诺 





图 5-35 将 球形 手腕 连接 至 关节 机 械 手 所 构成 的 关节 机 器 人 





组 装 复 合 刚体 总 是 有 一 些 附 加 的 坐标 系 。 人 额外 坐标 系 要 求 额外 变换 和 矩阵， 这些 变换 矩阵 
增加 了 所 要 求 数 学 计算 的 数量 。 建 议 在 连接 点 处 消除 neshin 坐标 系 ， 保 留 机 械 手 takht 坐标 
系 。 然 而 ， 只 要 已 知 坐 标 系 之 间 的 变换 和 矩阵， 产生 的 额外 坐标 系 并 不 是 一 个 显著 的 缺点 。 在 
图 5-35 中 ， 我 们 可 以 忽略 坐标 系 Bs ， 直 接 从 B: 变换 到 Bi, M 1; 二 1 十 19 取代 /gs 和 lo. 

例 5-32 组 装 的 平面 2R 机 械 手 。 

5-36 所 示 为 单 自 由 度 机 械 手 辟 ， 它 作为 一 个 R eR 平面 机 械 手 的 基 座 。 相 对 于 全 局 
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坐标 系 ， 该 机 械 臂 在 Mi 处 通过 电动 机 转动 ， 并 且 在 Ms 处 承载 另 一 电动 机 。 图 5-37 所 示 
为 一 个 平面 手腕 ， 用 于 连接 到 图 5-36 中 的 机 械 臂 上。 

5-36 中 的 坐标 系 Bo 是 机 械 手 takht 坐标 系 ， 图 5-37 中 的 坐标 系 Bs 是 手腕 neshin 坐 
标 系 。 组 装 手 腕 和 机 械 辟 构成 了 RR 平面 机 械 手 ， 如 图 5-38 Pra. 


























图 5-36 用 于 RR 平面 机 械 手 的 图 5-37 平面 手腕 
基 座 的 单 自由 度 机 械 臂 















































图 5-38 组 装 手腕 和 机 械 臂 所 形成 的 R1R 平 面 机 械 手 





例 5-33 ”组装 手腕 机 构 形 成 机 械 手 。 

考虑 一 个 机 器 人 ， 它 是 由 将 图 5-27 所 示 的 机 械 手 安装 到 图 5-22 所 示 关 节 臂 的 末端 点 所 
构 的 。 最 终 的 机 器 人 将 有 6 个 自由 度 ， 以 满足 能 到 达 在 其 工作 范围 内 不 同方 向 上 的 任何 点 的 
期 望 。 由 组 装 手腕 变换 矩阵 式 (5-118) 和 机 械 手 变换 矩阵 式 (5-74) HET OR f OL aie AGE Te 


运动 。 


























OT =T armT wrist T7 =° T33 Tsi T7 (5-136) 
手腕 变换 矩阵 ?Tr CAER (6-118) 中 给 出 ， 机 械 臂 变换 矩阵 "Ts 在 例 5-22 中 已 经 求 
得 。 然 而 ， 由 于 我 们 要 将 手腕 连接 到 坐标 系 Bs PHJ P Ab, I (5-119) 中 的 变换 矩阵 Ta 
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必须 包括 这 个 关节 距离 。 因 此 ， 我们 用 ?Ts 替换 式 〈5-119) 中 的 矩阵 。 





cOSO4 0 一 Sin04 0 
sind, 0 cosd 4 0 
3T, = (5-137) 
0 =l 0 ls 
0 0 0 1 
从 而 求 得 T wrist 为 
T wrist =’ Ts 
— sinf; sin06 十 cosO4 coss cosO6 ”一 Sin04cos06 — cosh, coss sins cosOusin05 0 
o cosl, Sin06 + cosh; cosg6 sinô, cosl coss — sinf, cosg5 sins sin0 sinf; 0 
— coshs Sin05 sings sinĝs cos; 13 
0 0 0 1 
(5-138) 
机 如 人 的 平衡 位 置 的 变换 矩阵 为 
1 0 lo 
0 1 d2 +l3 
T; = (5-139) 
0 0 1 di 十 Ci 
0 0 1 
为 
1 0 0 
0 1 0 
6T7 = (5-140) 
0 0 1 dr 
0 0 1 





例 5-34 球形 机 械 手 。 
球形 机 械 手 模仿 球形 坐标 系 ， 以 定位 3D 空间 中 的 一 点 。 图 5-39 所 示 为 一 个 球形 机 械 
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手 。 坐 标 系 Bo 是 机 械 手 的 全 局 坐标 系 或 者 基体 坐标 系 。 连 杆 〈1) SEA zo 轴 旋 转 ， 连 杆 
(2) 绕 着 垂直 于 zo 轴 的 z1 轴 旋 转 。 这 两 个 旋转 模仿 球形 坐标 系 的 两 个 角 运 动 。 径 向 坐标 由 











IT O 的 端点 处 有 一 个 takht 坐标 系 。 


连 杆 (3) 模仿 ， 连 杆 (3) 与 连 杆 (2) 之 间 具 有 一 个 棱柱 平 动 关节 。 在 与 手腕 相连 接 的 连 








图 5-39 中 的 连 杆 G) 是 一 个 REFR(90") 连 杆 ， 连 杆 (2) 也 是 一 个 RFR(C90") 连 杆 ， 


ÉI G) 是 一 个 Pl RO) 连 杆 ， 因 此 有 

















cos; 0 sind, 0 
sind} 0 一 cosO1 0 
oT 一 
0 1 0 d 
0 0 0 1 
cosG» 0 sinf22 0 
sind2 0 一 cosO， 0 
1T, = 
0 1 0 0 
0 0 0 i 
1 0 0 0 
0 1 0 0 
2T; = 
0 1 1 d; 
0 0 0 l 
从 坐标 系 Bs 到 坐标 系 Ba 的 变换 矩阵 仅仅 包含 一 个 平 动 d1: 
1 0 0 0 
0 1 0 0 
3T, 一 
0 1 T d4 
0 0 0 1 
从 坐标 系 By 到 基体 坐标 系 Bo 的 变换 矩阵 为 
T, 一 0T 1 1T, 2T33T， 
coOSOl cos@2 sind; cos; sind 2 (d3+d.4)cos@, sind, 
7 sinfı coso — cosi Sin0l sin0， (d3+d4)sin@; cos? 
sinĝ2 0 一 cOSO， d,—d3cos02—d.4cos2 
0 0 0 1 
在 平衡 位 置 ， 变 换 和 矩阵 "T4 可 简化 为 
1 0 0 0 
0 一 1] 0 0 
oT 一 
0 0 一 1] di 一 Qi 
0 0 0 1 





(5-141) 


(5-142) 


(5-143) 


(5-144) 


(5-145) 


(5-146) 





作为 常规 建议 ，D-H HERR AI E WB fA Ah BY Sak aE R AE PE AE — A> RE EB 
如 果 我 们 重新 调整 连 杆 CL) 的 坐标 系 使 得 它 成 为 一 个 RFR( 一 90") 连 杆 ， 这 时 "Ti 变 成 








198 





coOSO1 0 —sind, 0 
sinfı 0 cOSO1 0 
oT, 一 (5-147) 
0 =] 0 dı 
0 0 0 1 


并 且 在 平衡 位 置 处 的 综合 变换 和 矩 阵 变 成 了 一 个 特征 矩阵。 
为 了 使 连 杆 (1) 成 为 RFR( 一 90") 连 杆 ， 我 们 可 以 将 z1 轴 或 者 zl 轴 反 向 。 图 5-40 
所 示 为 坐标 系 的 新 排列 。 因 此 在 平衡 位 置 从 坐标 系 Bi 到 基体 坐标 系 Bo 的 变换 矩阵 简化 为 








1 0 0 0 
0 1 0 0 
oT, = (5-148) 
0 0 1 ci 十 Ca 
0 0 0 1 


























图 5-40 具有 在 平衡 位 置 将 综合 变换 矩阵 简化 为 实体 矩阵 的 坐标 系 的 球形 机 械 辟 


KG 5-35 将 球形 手腕 装配 到 一 球形 机 械 手 。 

为 将 图 5-40 中 的 机 械 手 变换 成 一 个 机 器 人 ， 我 们 需要 为 其 连接 一 个 机 械 手 。 我 们 将 例 
5-28 中 的 欧 拉 球 形 手 腕 连接 到 球形 机 械 手 上 。 手 腕 、 机 械 手 以 及 它们 的 附加 D-H 坐标 系 分 
别 如 图 5-28 和 图 5-40 所 示 。 

将 机 械 手 装配 到 机 械 臂 上 是 运动 回顾 ， 在 一 个 运算 中 ， 我 们 可 以 将 一 个 多 体 连接 到 其 他 
的 器 件 上 。 在 这 个 例子 中 ， 我们 将 一 个 球形 机 械 手 连接 到 一 个 球形 机 械 臂 上， 以 形成 一 个 球 
形 臂 -手机 器 人 。 

机 械 手 的 takht 坐标 系 By 和 机 械 辟 的 neshin 坐标 系 By 是 完全 相同 的 。 因 此 我 们 可 以 匹 
配 这 两 个 坐标 系 装配 机 械 手 和 机 械 辟 ,形成 一 个 组 装机 械 辟 -手机 器 人 ， 如 图 5-41 所 示 。 然 
而 ， 通 常情 况 下 ，takht 坐标 系 和 neshin 坐标 系 可 以 有 不 同 的 标记 ， 在 它们 之 间 有 一 个 常数 
变换 和 矩阵。 

FMA bs A Bo 的 正 向 运动 可 由 和 矩阵 相 乘 求 得 

0T9 一 07T11722T33T44T557T66T77T88T9 (5-149) 
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图 5-41 球形 机 械 手 与 机 械 辟 的 组 装 装 配 


FMT; 在 例 5-28 和 例 5-34 中 已 经 给 出 。 

我 们 可 以 消除 坐标 系 B 和 Bs 以 减少 总 的 坐标 系数 目 ， 并 旦 简化 矩阵 运算 。 然 而 ,我 
们 可 以 改变 具有 新 坐标 系 的 手腕 ， 简 化 组 装 过 程 。 如 果 假 设 这 个 组 装机 器 人 需要 工作 一 会 
儿 ， 那 么 我 们 可 以 消除 和 简化 机 器 人 的 变换 和 矩阵， 使 其 成 为 一 个 变换 矩阵 ， 如 图 5-42 所 示 。 
在 数学 上 ， 我 们 应 该 用 变换 矩阵 2Ts 替代 消除 掉 的 坐标 系 Bs 和 B. 











1 0 0 0 

0 1 0 0 
2T; =? T33 TitT; = (5-150) 

0 0 1 ds 

QO. 0 0 1 
ds =d; +d, +d; (5-151) 

ME, PIEPRZ Bo 的 正 向 运动 变换 变 为 

oTo =° Ti1T,?: Ts Te Tr’ Tes To (5-152) 











图 5-42 简化 球形 机 械 手 和 机 械 臂 组装 坐标 系 


例 5-36 球形 机 器 人 正 向 运动 学 分 析 。 
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5-43 所 示 为 连接 到 球形 手腕 的 一 个 球形 机 械 手 ， 以 形成 RFREFP 机 器 人 。 相 关联 的 
D-H 参数 见 表 5-13。 


表 5-13 Stanford HL HAY D-H 参数 




















坐标 系 编号 ai a; di 0; 
1 0 —90° 0 01 
2 0 90° l2 0 
3 0 0 d 0 
4 0 —90° 0 04 
9 0 90° 0 0 
6 0 0 0 6 























图 5-43 组 装 球形 机 械 手 和 球形 手腕 所 形成 的 球形 机 器 人 


然而 ， 我 们 推荐 应 用 例 5-9 一 例 5-14 中 的 连 杆 关节 分 类 。 连 杆 -关节 组 合 见 表 5-14。 
表 5-14 连 杆 -关节 类 型 

















连 杆 号 类 型 连 杆 号 类 型 
1 RER(—90°) 4 RER(—90°) 
2 REP(90°) 5 RER(90°) 
3 P || RCo’) 6 R || R09 

















应 用 相关 联 的 变换 矩阵 将 坐标 系 B; 变换 至 坐标 系 B;-1， 则 有 


coOSO1 0 一 Sin01 0 
sind; 0 cos 0 
oT = (5-153) 
0 =] 0 0 
0 0 0 1 
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cos0» 0 sind» 0 
sinfg 0 一 cos0， 0 
IT, = 
0 1 0 L2 
0 0 0 1 
1 0 0 0 
0 1 0 0 
2T, = 
0 0 1 dz 
0 0 0 
coOSO14 0 一 Sin04 0 
sinĝ4 0 COSO4 0 
3T, 一 
0 — |1 0 0 
0 0 0 1 
cosO5 0 sinds 0 
sinds 0 一 cos0O 0 
tT; = 
0 1 0 0 
0 0 0 1 
cos0O6 ”一 Sin06 0 0 
sins coség 0 0 
“Te = 
0 0 1 0 
0 0 0 1 





这 里 


r21 r22 T23 r24 


rıı = sinl (— cosl, sinfı — cosé; cos@2 sinb, ) 


+ cosĝs L — cos: sinĝ2 sin05 十 cos05 (一 sin0l sin04 十 cosgl cos62 cosg4 ) | 


721 一 Sin06(cosOl cosO4 —cos2 sind; sin04 ) 





r31 =sin§s sind, sinds 4 


十 cos0iL 一 sin0l sin0y sin05 十 cos05 (cos01 sin04 +cos02cos64sind, ) | 


FcosOe ( — cos@2 Sin05 — cosl, coss sinf?) 


ri2 一 COSO6( 一 cosO4 Sin01 一 cosOl cos@2 sinG 4 ) 


— sin9s [| 一 cosOl sin0y sin05 十 cos05 (一 cosOl sind, 十 cosOl cosd2cosd, ) | 





r22 =cosls (cosh cos04 — cos2 sind; sin04 ) 


— sings | 一 sin0i sin0ysin0i + cosOs (cosh: sin04 十 cosOy cosd, sind, ) | 


r32 =cosd¢ Sin02 Sin04 


十 sin06 (cos@2 sin05 十 cosOu coss sin0， ) 


r13 一 COSOl cos; sinf 十 Sin05( 一 Sin0l sinf, 十 cosOl cosOx cosO4 ) 


r23 =cosds sinfı Sin0， 
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十 Sin05(cosOl sin04 十 cosOl cos, sing, ) 


(5-154) 


(5-155) 


(5-156) 


(5-157) 


(5-158) 


(5-159) 


(5-160) 


(5-161) 
(5-162) 


(5-163) 


(5-164) 
(5-165) 
(5-166) 
(5-167) 
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r33 =cos02 coss — cosh, sinf sinl; (5-168) 
ri4=—l2sinO; +d cos sins (5-169) 
r24, =Ll2cos0; +d3sind) sin02 (5-170) 
r34=d3cos02 (5-171) 
FAY FFB Oo Ts 乘 以 手 爪 坐标 系 Bz HAEE E RIER Ai FAT ie BY ZE RE EO 
277 一 "T65T7 (5-172) 
这 里 
1 0 0 0 
êT: 一 ea (5-173) 
0 0 1 dr 
0 0 0 1 


例 5-37 EAT AL ait A AE RTE 

H T RAH TA an R PSY AB os E Ke E SE AS A os RHP RAE He FE EME, RA 
BY LA ZEAE OF 5 ie Ee, LER ATT AC 5-36 中 所 分 析 的 球形 机 器 人 
关节 旋转 角度 。 











6,=0, 02=0, 6,;=0, 04=0, 6;=0 (5-174) 
因此 ， 变 换 式 (5-159) 将 得 





1 0 0 0 
. 0 1 0 Le 
076 王 "TI17T22T33T414T557T6 一 (5-175) 
0 0 1 d} 
0 0 0 1 
式 (5-175) 恰好 说 明了 在 机 器 人 坐标 系 中 手 爪 坐标 系 的 原点 
0 
Gros =| l2 (5-176) 
d3 


15.6 ”螺旋 坐标 变换 





有 可 能 用 螺旋 运动 描述 两 相 邻 坐标 系 B; 和 B;-1 之 间 的 变换 矩阵 。 可 以 通过 中 心 螺 旋 运 
Bs (ais ais ti) 和 另 一 个 中 心 螺 旋 运 动 y (di, Oi. ki) XM, 
17; = fdisk ima 8 wees 











cos?; — sinĝ; cosa; sind; sina; a;cosd; 
sind; cos0; cosa; —cos0;sina; a;sind; 
= | (5-177) 
0 sina ; cosa; di 
0 0 0 1 


证 明 : 
中 心 螺旋 运动 (a;，a;，ii-1) 变换 矩阵 为 
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(Qaisai37i1)—=D Cla; sii) RC i—1 30:1) =D zra; R iisa 


1 0 0 a;\f1 0 0 0 0 0 a; 
|010 0 0 cosa; —sina; 0 = |O cosa; —sine; 0 
7 0 0 1 0|10 singi cosa ; 0 = 0 sina; Cosa ; 0 

0 0 0 1/\0 0 0 1 0 0 0 1 


Fa— PUD IR PEI AY (di. 0i, Ri) 的 变换 矩阵 为 


£4: 0; sh p=1) =D di sh 1 RG; 1,0;)=D,, 1,, Re 











1 0 0 O)\fcosé; —siné; 0 0 cos0; 一 Sin0; 
_10 1 0 O |) sind; cos0; 0 of sind; cosĝ; 
lo 0 1 ad; || o o 1 0] | 0 0 

0 0 0 1 0 0 0 1 0 0 

Ak, FE BN SHER The as oh For AE AY Be ie ET 为 
1D; = (dj 0; Ri 1)s (ai saisti) 
cos0; —sin#é; 0 0\/1 0 0 
= | sind; cos0; 0 0 10 cosa; —sina; 
|o 0 1 di|I0 sina; cosa ; 
0 0 Oo 1/\0 0 0 
cos; —cosa;sind; sind; sina; a;cos0; 
| sind; cos0; cosa; —cos@;sina; a;sind; 
B 0 sina; cosa i di 
0 0 0 1 


0; 
0 0 
0 
l di 
O I 
a; 

0 

0 

1 


(5-178) 


(5-179) 


(5-180) 


AR He SE MET, 等 效 于 常规 螺旋 运动 ， 可 基于 式 (4-161) 和 式 (4-162) 求 得 相关 参数 。 





基于 式 (4-165)， 可 以 计算 螺旋 运动 的 旋转 角 g: 














1 、 1 
cosg =z Ltr(CRsg ) 1]= Ccosð, + cos0; cosa; +cosa; —1) 
螺旋 轴 立 可 以 通过 式 (4-167) 求 得 : 

一 一 一 一 (SGRB 一 cRT) 

2sing B 

cos; — cosa; sinl; sind; sina; cosĝ; sind; 
1 . . 

二 一 一 | | sin0; cosĝ; cosa; 一 cosO sina; | 一 | —cosa; sind; cosl; cosa; 

Zsing 

0 sina; cosa; sing; sina; — cosl; sina; 
0 — sinl; —cosa; sind; sind; sina; 
1 ; : . ; 

=— sinl; +cosa; sind; 0 — sina; 一 cosOi sina ; 

2sing 

一 Sin0; sina; sina; 十 cosO; sina; 0 
因此 ， 有 
sina; 十 cosO; sina ; 
1 ‘ s 
ú = | sind; sina; 
2sing 





Sin0; + cosa; sind; 
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(5-181) 


0 
sina ; 


COSR; 


(5-182) 


(5-183) 
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平移 参数 h 和 螺旋 轴 上 一 点 的 位 置 矢量 , 例如 (0 y zi-1)， 可 通过 式 (4-170) 


h ui Tr: 一 r13 \ tru 
Yyi—ı |5]uz2 1 一 r22 =rz3 r24 
z=] Wy —r32 1—r33 r34 
sina; + cos0; sina; — sind; cosa; sinĝ; sina; )~! (a;cos6; 
1 . . ; 
一 一 一 一 sind; sina ; 1—cos0;cosa; —cos0; sina; a;sing; 
Zsing 
sind; + cosa; sind; sina; cOSa; d; 





(5-184) 
友 例 5-38 工业 机 器 人 连 杆 的 螺旋 运动 分 类 。 
工业 机 器 人 上 最 常 使 用 12 种 不 同 的 配置 。 每 种 类 型 都 有 其 本 身 的 几何 配置 和 变换 的 类 
别 。 每 种 类 别 部 可 在 连 杆 的 两 个 末端 处 的 关 方 相 辨 别 ， 并 且 有 从 末端 关节 坐标 系 B; 变换 到 
近 端 关节 坐标 系 B;-; 的 变换 矩阵 。 每 个 种 类 的 变换 矩阵 唯一 地 取决 于 近 端 关节 和 > 轴 之 间 
的 角度 。 对 于 两 个 任意 坐标 系 ， 其 螺旋 运动 表达 式 为 









































1T;=s (di;, Oi, kii)S(a;, ais 1i-1) (5-185) 
这 里 

Cdi, Ois Ri-1)=D(di, ki-1)R(Ri-1, 0;) (5-186) 

Sla; ais ti-1)=DCa;, ti—1)R(fi—-1; ai) (5-187) 








依据 表 5-15， 每 种 的 坐标 系 变换 螺旋 表达 式 可 以 被 简化 。 
表 5-15 工业 机 器 人 连 杆 的 螺旋 分 类 






































序号 连 杆 类 型 17; 

1 R || RCO?) BK R || PCO?) %0,0: Rk DY (ai 01) 
2 R || RC180°) aK R || P180% $0.0; shi-1) ¥ Cai 2m sti-1) 
3 R_| R90") B R| P(90°) £050; Ei) Caism, ti) 
4 RLR(—90°) # R| P(— 90°) $0.0; Ri DY Cais nsii) 
5 R FR(90°) a R FP(90°) y(0,0 shi-1) ¥ CO, r,t; 1) 
6 R FR(—90°) #& R FP(—90°) $0.0; hi) (0, — rii) 
7 P || RC0® 3} P || PCO?) ¥(di.0,k;-1)8 (aj 0t;-1) 
8 P || RC180°) # P || P(180°) ydis0R Dyas,2n,ti 1) 
9 P| R(90”) 或 P| P(90°) ydis0,R DY (aj nti 1) 
10 P| R(— 90°) 3} P| P(— 90°) ¥(d; 50,81) ¥ lais — n, ii—1) 
11 P FR(90°) Be P HP(90°) $di,0,R 1 (0,n,ti 1) 
12 PFR( 一 90”) 或 PFP( 一 90") ¥(d;,0,8;-1)¥ (0,—m,7;-1) 




















作为 一 个 示例 ， 我 们 可 以 检查 第 1 类 。 
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i—IT; =$(0, 0;, ki;_iS(a;, 0, fi-1) 
一 D(0， Ri DR: =, 0; D(a,;, i RG; 1, 0) 


cosO; —sing; 0 O/A 0 0 a; 
| sind; cosO 0 OJO 1 0 0 
fo o 1 olf0 0 1 0 

0 0 0 1/0 0 0 1 

cos#; —siné; 0 a;cosd; 
| sind; cos; 0 a;sind; j 
7 0 0 1 

0 0 0 




















结果 与 式 (5-32) 中 结果 是 相同 的 。 
ne 球形 机 器 人 螺旋 正 向 运动 学 。 
通过 确定 每 根 连 杆 的 种 类 和 应 用 相关 联 的 螺旋 可 以 将 螺旋 运动 应 用 于 正 向 运动 学 之 中 。 
如 图 5-43 所 示 的 球形 机 器 人 的 连 杆 分 类 ， 其 说 明 见 表 5-16. 
因此 ， 球 形 机 器 人 的 末端 执行 器 在 基体 坐标 系 中 的 坐标 变换 为 
OT 一 0TiIT22T33T447T55T6 =S (050; Ři—1)S (Oy — 11-18 (050; ki 1)$ (0,n,ti 1)X 
© Cd 5 0shia 8 Cay Ori) 0,0; bi) 8 0, — 1) X 
$050; »Ri—1 Onsii 1)8 (00; »Ri—1) 8 (a; ,0,7i 1) 















































(5-188) 
表 5-16 图 5-43 所 示 的 球形 机 器 人 的 螺旋 变换 
连 杆 号 分 类 螺旋 变换 
1 R FR(—90°) OTi =$ (0,0; 8-1) (0, — xfi) 
2 R FP(90°) 1D, = (0,0; 9hj-1)¥ (Os tj-1) 
3 P || R) 213 =% (di 0, ki) Cais0sii—1) 
4 R FR(— 90% 97, =F (0,0; 2:1) ¥ (0, — my t-1) 
5 R FR(90°) 1T; =37 (0.0; 8-18 (0,n,ti1) 
6 R || RO) 515 =8 (0.0; Bi 1) 8 (a; 0571-1) 
KI 5-40 Pub ez oh eB oe he 
利用 普 吕 克 坐 标 ， 也 可 以 定义 中 心 螺旋 运动 。 
Whe th a) =| 四 (5-189) 
式 (5-189) 等 于 
[池上 = DihwR(u, $) (5-190) 
六 例 5-41 中 心 螺旋 运动 y (a;，a;， 2-1) WÉ B WEER. 
中 心 螺旋 运动 ¥ (ajs ai. 2-1) 也 可 以 用 一 个 恰当 的 普 如 克 坐 标 表述 。 
$a, ais a =p, Pip aia td al 
Qa ;li-] 
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类 似 地 ， 中 心 螺 旋 运 动 y Cdi, Ois di1) 也 可 以 由 一 个 恰当 的 善 虽 克 坐标 表述 


biki 


$(d; 50; én f (5-192) 


|- D(Cdi RD)RCGR 1,0;) 
iRi-1 


KB 5-42 ”相互 交叉 的 中 心 螺旋 运动 。 
如 果 两 个 中 心 螺旋 运动 的 相互 又 积 是 零 ， 那 么 这 两 个 中 心 螺 旋 运 动 是 相互 交叉 的 。 我 们 





可 以 检查 两 中 心 螺 旋 和 运动》 (a;> Qi» fi_1) All s (di, Urs ki-1) 的 交互 积 是 否 为 零 。 
_ 、 O:Ri—1 QiLi—1 
S(d; 0; +ki-1)XSCa;, Qislj-1)= dÈ. ， & a (5-193) 


=0Ri 1 * ajii-1 Haili * O:Ri-1 =0 


*5.7 非 D-H 法 








分 配 机 器 人 连 杆 相关 坐标 系 的 D-H 法 是 最 和 常用 的 方法 。 然 而 ，D-H 法 并 不 是 唯一 使 用 
的 方法 ， 也 不 是 最 好 的 方法 。 与 D-H 法 相 比 ， A 

Sheth 法 就 是 一 个 可 以 选择 的 方法 ， z; 
它 建立 坐标 系 的 数目 等 于 连 杆 上 关节 的 数 
目 从 而 克服 了 D-H 法 对 高 阶 连 杆 的 限制 ， 
对 于 求解 连 杆 几何 尺寸 更 加 灵活 。 

在 Sheth 法 中 ,我 们 在 连 杆 的 每 个 关 
节 处 分 别 定义 了 一 个 坐标 系 ， 因 此 一 个 具 
An 个 关节 的 机 器 人 具有 2n 坐标 系 。 图 
5-44 给 出 了 连 杆 G) 的 情况 ,第 1 个 坐 
ma (x;5 yis zi) 连接 在 连 杆 的 始 端 ， 

第 2 个 坐标 系 (Cui. vir wi) 连接 至 连 
杆 的 末端 。 连 杆 的 始 端 关 节 和 末端 关节 的 
分 配 是 任意 的 ， 然 而 如 果 它 们 是 治 着 基 
体 - 手 爪 坐标 系 的 方向 ， 那 么 相应 坐标 系 
的 建立 就 容易 多 了 。 

为 了 描述 几何 特性 ， 首 先 我 们 用 z; 
AA w: 轴 定 义 关节 轴 ， 然 后 确定 这 两 个 


























DA 3 图 5-44 定义 两 副 杆 上 始 端 坐 标 系 和 
关节 轴 z; 轴 和 w; 轴 的 公法 线 。 公 法 线 末端 坐标 系 的 Sheth 法 


可 用 单位 矢量 %; 来 表示 。 确 定 连 杆 的 几 

何 特性 要 求 有 6 个 参数 ， 上 具体 如 下 : 
1) a; 是 从 >; WE w: 轴 且 沿 着 公法 线 7i; 的 距离 ， 它 是 zx; MA ws 轴 之 间 的 运动 距离 。 
2) b; EMAC; Blu: 轴 且 沿 着 w; 轴 的 距离 ， 它 是 ww; 轴 的 标高 。 
3) c; 是 从 xz; 轴 到 公法 线 7i; 且 沿 着 z; 轴 的 距离 ， 它 是 xz; 轴 的 标高 。 




















4) ai 是 zx; 轴 和 w; 轴 所 形成 的 夹 角 ， 绕 着 公法 线 ni 从 z; WE ws 轴 的 正 角 。 
5) Bi AWA ts Mu: 轴 所 形成 的 夹 角 ， 绕 着 wi MACE n: 到; 轴 的 正 角 。 
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6) V: 是 z; MAMA n: MERKRA, RE x; WA c: 轴 到 公法 线 n: 的 正 角 。 
Sheth 参数 产生 了 一 个 齐 次 变换 矩阵 


Te=°T.ela;s bis Cis ais Bis 7;) (5-194) 

以 便 将 末端 坐标 系 Be 变换 到 始 端 坐标 系 Bo 

Ti Uj; 

Yi jop] (5-195) 

zi wi 

1 1 
这 里 ，"T。 表 示 Sheth 变换 矩阵 。 于 是 

ri F132 ris ri4 


T= i ° (5-196) 





这 里 
rıı =cosf; cosy; — cosa; sinf; siny; (5-197) 
r21 =cosf; siny; + cosa; cosy; sing; (5-198) 
r31 =sina; sinf; (5-199) 
r12 = — cosy; sinB; — cosa; cos; siny ; (5-200) 
r22 = — sinf; siny; + cosa; cos; cosy; (5-201) 
r32 =cosf; sina; (5-202) 
r13 = sina; siny; (5-203) 
r23 =— cosy; sina; (5-204) 
T33 =COSQ i; (5-205) 
ri4 =a; cosy; +6; sina; siny; (5-206) 
r2, =a; siny; — b; cosy; sina; (5-207) 
r34 =c; +6, cosa; (5-208) 
关节 处 两 个 坐标 系 的 Sheth 变换 和 矩阵， 对 于 棱柱 关节 可 简化 为 平 动 ， 对 于 旋转 关节 可 简 
化 为 绕 着 Z 轴 的 旋转 。 
UE AA : 
当 坐 标 系 B; 中 的 坐标 给 定时 ， 章 次 变换 矩阵 为 
"Te=D 2, ,¢,Re,.7,D 2;,0,R r; ra; D zibi R zp; (5-209) 
应 用 相关 的 变换 矩阵 : 
cosB; —sinB; 0 0 
ra A S (5-210) 
0 0 1 0 
0 0 0 1 
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1 0 0 0 
D an (5-211) 
Pe Nie. ly A b g 
0 0 0 1 
1 0 0 0 
0 cosa; 一 Sinai 0 
Ra, = (5-212) 
0 sina; cosa; 0 
0 0 0 1 
1 0 0 a; 
0 1 0 0 
Dza; = (5-213) 
0 0 1 0 
0 0 0 1 
cosy; —siny; 0 0 
siny; cosy; 0 0 
R.,,y, = (5-214) 
0 0 1 0 
0 0 0 1 
1 0 0 0 
0 1 0 0 
D...,= (5-215) 
0 0 1 Ci 
0 0 0 1 
我 们 可 以 证 实 ，Sheth 变换 矩阵 为 
cosBicosy; — cosy; sing; , , , l 
, , , sina;siny; a;cosy; Fb; sina; siny; 
— cosa; sinf; siny; — cosa; cosp; siny; 
cosß; siny; —sinf; siny; l , 
, — cosa; cosy; a,sinY; —b; cosy; sina; 
+cosa;cosy;sinB; + cosa; cosp; cosy; 
sina; sinf; sina; cospi cosg i c; Fb; cosa; 
0 0 0 1 


(5-216) 


KB 5-43 ”旋转 关节 和 楼 柱 平 动 关节 的 Sheth 变换 矩阵 。 





由 旋转 关节 所 连接 的 两 连 杆 如 图 5-45a 所 示 。 在 两 连 杆 的 公共 关节 处 建立 坐标 系 ， 以 便 

















z; WA w; 轴 跟 旋转 轴 一 致 ， 这 两 个 坐标 系 具 有 相同 的 原点 。Sheth BM: a=0, b=0, 








c=0, 


Qa 0, B 0, y a, 因此 ， 有 

cos? — sin 
(5-217) 
0 0 


0 0 
sind cosO 0 0 
1 0 
0 0 0 1 





由 棱柱 平 动 关 节 所 连接 的 两 连 杆 如 图 5-45b 所 示 。Sheth 变量 是 沿 着 关节 轴 的 4d。 在 两 








连 杆 上 





的 公共 关节 处 建立 坐标 系 ， 以 便 z, 轴 和 wj; 轴 跟 平 动 轴 一 致 ，z; 轴 和 wj 在 同一 方向 上 
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a) b) 
图 5-45 旋转 关节 和 棱柱 平 动 关节 所 连接 的 两 连 杆 
a) 旋转 关节 b) 棱柱 平 动 关节 


B=0, y=0, Alt. @ 




















平行 。 Sheth 参数 . a 0, b 0, C d, Qa 





100 0 
. 0 1 0 0 
iT; = (5-218) 
* lo 0 1d 

0 0 0 1 


克 例 5-44 圆柱 关节 的 Sheth 变换 矩阵。 
圆柱 关节 具有 两 个 自由 度 ， 分 别 是 沿 着 同一 轴 的 
旋转 运动 和 平移 运动 。 由 圆柱 关节 相连 接 的 两 连 杆 如 
图 5-46 所 示 。 圆 柱 关节 的 变换 矩阵 可 用 一 个 旋转 关节 
和 一 个 棱柱 平 动 关 节 的 组 合 进行 描述 。Sheth 参数 : 
a=0,6=0, c=d, a=0, B=0, y=@. MU, A 
cos0 一 Sin0 0 0 
ees sind cos? 0 0 
TIX) 9 0 1d (5-219) 图 5-46 由 圆 桂 关 节 相连 接 的 两 连 杆 
0 0 0 1 
KB 5-45 螺旋 关节 的 Sheth 变换 矩阵 。 
如 图 5-47 所 示 ， 螺 旋 关 节 可 提供 一 个 成 比例 的 旋转 运动 和 平移 运动 ， 它 有 一 个 自由 度 。 
平 动 位 移 h 和 旋转 角 9 的 比 称 为 螺 距 ， 定义 为 


=y (5-220) zw 
螺旋 关节 的 变换 可 以 依据 相对 旋转 角 0 表述 为 


cosO 一 Sin0 0 0 


sin@ cosO 0 0 
iT; = (5-221) x; 












































0 0 1 pé 一 
0 0 0 1 So uy 
或 者 依据 位 移 /表述 为 图 5-47 ”螺旋 关节 所 连接 的 两 杆 
cos(h/p) —sin(h/p) 0 0 
:T= sin(h/p) cos(h/p) 0 0 (5-299) 
0 0 l] h 
0 0 0 1 
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在 螺旋 关节 所 在 的 相互 连接 的 两 连 杆 上 建立 坐标 系 可 以 使 得 在 平衡 位 置 处 w 轴 和 >， 











轴 沿 着 螺旋 轴 对 齐 ，xj BAM; 轴 一 致 。 

KI 5-46 齿轮 关节 的 Sheth 变换 矩阵 。 

Sheth 法 也 可 用 来 描述 由 齿轮 关节 相连 接 的 两 连 杆 的 相对 运 
动 。 齿 轮 关 节 ， 如 图 5-48 所 示 ， 它 提供 了 一 个 成 比例 的 旋转 运 
动 ， 有 一 个 自由 度 。 旋 转轴 是 w; 和 >; W, HAERERE n. 
这 时 ，Sheth 参数 可 定义 为 : a 一 R; 十 R;, 5 二 0, c=0, 以 及 
a 二 0，B 二 0;，7Y 二 0;。 因 此 ， 有 



































cog(1 十 s)0; —sin(l1+e)d; 0 有 Ri(L 十 es)cos0， 
ig | Sin +e); cos(1+e)6; 0 Rj (1+e)siné; 
aa 0 0 1 0 
0 0 0 1 
(5-223) 





=i (5-224) Ga) a 
* Gl 5-47 H-R 法 和 D-H 法 的 奇异 性 。 
E D-H AF, WARA FIT, JF 
没有 很 好 地 定义 公法 线 。 在 这 种 情况 下 ，D- 
H 法 有 奇异 性 ， 因 平行 关节 轴 在 空间 位 置 中 
的 一 个 很 小 的 变化 能 够 引起 在 其 相对 位 置 的 
D-H 坐标 表述 中 的 一 个 大 变化 。 
Hayati-Roberts (H-R) 法 是 另 一 种 表达 
顺序 连 杆 的 方法 。 对 平行 轴 的 情况 ，H-R 法 
避 开 了 坐标 的 奇异 性 。 在 H-R 法 中 ，B;-1 坐 
标 系 中 z; 轴 的 方向 通过 翻滚 角 os 和 俯仰 角 


























Bi 所 定义 ， 如 图 5-49 Pras. BAB 的 原 ”图 5-49 避 开 D-H 法 中 奇异 性 的 H-R 法 标记 





点 选择 在 zx;-1y;-1 平 面 内 ， 在 该 平面 中 距离 
di 可 由 原点 o;-1 和 o; 之 间 的 距离 所 得 。 

















与 D-H 法 定义 相 类 似 ，H-R 法 涉及 了 在 选择 旋转 角 过 程 中 的 自由 度 。 而 且 ， 虽然 H-R 
法 可 以 消除 平行 关节 轴 的 奇异 性 ， 但 是 当 z; 轴 平 行 于 x;-1 轴 或 者 y;-1 轴 以 及 当 =; 轴 与 坐 





bp A B;-1 的 原点 相交 时 它 有 其 自己 的 奇异 性 。 
KI 5-48 参数 化 的 连续 定义 法 。 

















还 有 另 一 种 用 于 坐标 变换 的 方法 ， 称 之 为 参数 化 连续 定义 (Parameterically Continuous 


convention, PC), PC 法 通过 以 下 两 个 步骤 表达 z; W: 


1) 相对 于 x;-1 轴 和 yy;-1 轴 ，z; 轴 的 方向 由 a; 和 8B; 的 方向 余弦 所 定义 。 
2) z; 轴 的 位 置 由 两 个 位 移 参 数 1; Mm: 所 定义 ， 这 两 个 参数 表示 了 z; W xyi 


平面 交点 的 zx;-1 和 yi SBD 
从 Bi;-1 坐 标 系 变换 到 B; 坐标 系 的 PC 齐 次 变换 矩阵 为 
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a; Qi 
ay lay, 
; 
17, = ge ae Bi mi (5-225) 
—a =p y 0 
0 0 0 1 
这 里 
¥,=/1—a? —p? (5-226) 


PC 法 利用 4 个 参数 说 明 坐 标 系 B;-1 在 坐标 系 B 中 的 位 置 和 方向 。 通 常情 况 下 ， 我 们 
需要 6 个 参数 ， 因 此 必须 有 以 下 两 个 条 件 : 

1) 坐标 系 B: 的 原点 在 公法 线 上 ， 并 且 沿 着 关节 轴 。 

2) x; 轴 必 须 沿 着 公法 线 。 








5.8 本 童 小结 


当 关节 变量 给 定时 ， 正 向 运动 学 确定 每 根 连 杆 ， 特 别 是 未 端 执 行 器 、 坐 标 系 在 机 名 人 基 
体 坐 标 系 中 的 配置 。 








orp =° T," rp (5-227) 
对 于 由 根 连 杆 串联 的 机 器 人 来 说 ， 相 当 于 寻找 变换 矩阵 "T， 作 为 关节 变量 wo; 的 函数 。 
OT, =° T: (q1) T2 (Cdq2)27T3(0d3)3T4(004) 2 1T, (gn) (5-228) 


对 于 在 机 器 人 连 杆 上 建立 坐标 系 的 特殊 规则 ， 将 其 称 之 为 D-H 规则 。 基 于 D-H 规则 ， 
从 坐标 系 B; 变换 到 坐标 系 Bii 的 每 个 变换 矩阵 一 Ti; 可 由 4 个 参数 表示 : 连 杆 长 度 a;、 连 
杆 旋 转角 a;、 关 节 距 离 di 和 关节 角 0;。 











cos; —sin0;cosa; sinĝ; sina; aicosOi 
_ sind; cosOicosai = —cosd;sina; a;sind; 
i 一 1T ;一 (5-229) 
0 sina ; cosa; di 
0 0 0 


然而 ， 对 于 大 部 分 工业 机 器 人 来 说 ， 连 杆 变换 矩阵 一 :T 可 以 被 分 为 12 种 简单 类 型 ， 
具体 类 型 见 表 5-6 。 
大 部 分 工业 机 器 人 由 一 个 配备 3 自由 度 球形 手腕 的 3 自由 度 机 械 手 所 构成 。 变 换算 
REOT? 可 以 被 分 解 为 以 下 3 SFE OTS ° Ts MIST? 。 

OT: 一 0T337T6567T7 (5-230) 

和 矩阵 "Ts 确定 手腕 点 的 位 置 ， 它 只 取决 于 机 械 手 关节 变量 。 和 矩阵 Te 是 手腕 变换 ， 它 只 
取决 于 机 械 手 的 手腕 变量 。 常 量 矩 阵 *T;? 是 手 爪 变换 矩阵 。 将 矩阵 "Tz 分 解 为 子 和 矩阵 可 使 我 
们 将 正 向 运动 标准 化 。 






































习题 
5-1 标记 与 符号 。 
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描述 下 列 这 些 符号 的 含义 。 





1) 0; 2) ai 3) R || R(180°) 

6)'-!T; 7) & 8) §(a; sa; 52i-1) 
@iti-1 

11) d; 12) a; 13) | i | 
Qili—l 

5-2 4R 平 面 机 械 手 。 

AR 平面 机 械 手 如 图 5-50 所 示 ， 试 求 : 

1) D-H 参数 表 。 

2) 连 杆 关节 表 

3) 每 个 坐标 系 变换 矩阵 全 1T; 1=1,2,3,4, 

4) 末端 执行 器 的 全 局 坐标 。 

5) 末端 执行 器 的 方位 © 


5) R FR(—90°) 
10) $ (di;,0;,k; 1) 


| 


4) P+R(—90°) 
9) sth,$d.a) 


Oiki_ 1 
| ey 
ú diki=l 


gu 
14) f 











图 5-50 4R 机 械 手 


5-3 IF R FR( 一 90?) 机 械 辟 。 





对 于 所 图 5-51a 和 图 5-51b 所 示 的 单 连 杆 R 上 FR( 一 907) 机 械 手 ， 完 成 以 下 命题 





HFF R FR( 一 90") 机 械 手 
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1) 试 求 变换 矩阵 "Ti T: MT. 

2) 比较 两 个 视图 中 的 变换 矩阵 IT， 。 

5-4 2R 平面 机 械 手 。 

对 于 图 5-52 和 图 5-53 所 示 的 2R 平 面 机 械 手 ,确定 其 中 的 连 杆 变 换 和 矩阵 1Ts ?Ts MT. 





























V3 


2 








图 5-52 具有 D-H 坐标 系 的 2R 平面 机 械 手 

















TH 





图 5-53 具有 任意 坐标 系 的 2R FLORA 








5-5 2R- 平面 机 械 手 。 

对 于 图 5-52 所 示 的 2R 平面 机 械 手 ， 请 确定 变换 矩阵 3T。 .2T1 APT) 

5-6 建立 D-H 坐标 系 。 

对 于 图 5-16a 和 图 5-16b 中 的 机 械 手 ， 请 建立 所 要 求 的 连 杆 坐标 系 。 

5-7 “对 于 被 连接 的 连 杆 ， 建 立 D-H 坐标 系 。 

1) 对 于 图 5-54a 和 图 5-54b 中 的 机 械 手 ， 利 用 连 杆 的 长 度 1 、4 、/s 建立 所 要 求 的 连 杆 坐 








标 系 。 

2) 确定 图 5-54a 和 图 5-54b 中 机 械 手 的 正 向 运动 变换 矩阵 ， 并 求 它 们 的 静止 〈 平 衡 ) 
位 置 。 

3) 确定 图 5-54a 和 图 5-54b 中 所 示 机 械 手 末梢 点 所 在 位 置 的 全 局 坐标 。 








5-8 ”中 心 处 的 坐标 系 。 
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a) b) 
图 5-54 由 工业 连 杆 所 构成 的 两 个 机 械 手 


在 连 杆 的 几何 中 心 a;/2 处 建立 连 杆 坐标 系 。 对 于 图 5-55 中 的 机 械 手 ， 利 用 刚体 运动 和 
齐 次 矩阵 确定 变换 矩阵 "Ti T AT 。 














图 5-55 在 每 根 连 杆 几何 中 心 处 建立 坐标 系 的 2R 平面 机 械 手 








5-9 极 坐 标 机 械 手 。 

对 于 如 图 5-56 中 所 示 的 极 坐 标 机 械 手 ， 确 定 连 杆 变换 矩阵 1T? Ts AT 。 

5-10 ”替换 极 坐 标 机 械 手 的 基体 和 末端 执行 器 。 

对 于 如 图 5-56 中 所 示 的 机 械 手 ， 确 定 连 杆 变换 矩阵 ?了 >， 、 2T MT. 

5-11 平面 笛 卡 儿 机 械 手 。 

对 于 如 图 5-57 中 所 示 的 平面 稍 卡 儿 机 械 手 ， 确 定 连 杆 变换 矩阵 1T: T 和 1T3。 提示: 
坐标 系 并 不 是 建立 在 D-H 规则 上 的 。 

W5-12 机械手 设计 。 

利用 工业 机 器 人 连 杆 制 造 一 个 机 械 手 ， 

D 有 3 个 平 动 关 节 ， 并 且 到 达 在 三 维 笛 卡 儿 空 间 中 的 每 个 点 。 




















r 
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2) 有 两 个 平 动 关节 和 1 个 旋转 关节 ， 并 且 到 达 在 三 维 笛 卡 儿 空 间 中 的 每 个 点 。 


Zn g 
22, "3 








eT 


























图 5-56 2 自由 度 极 坐 标 机 械 手 图 5-57 2 自由 度 笛 卡 儿 机 械 手 
3 在 三 维 笛 卡 儿 空 间 中 ， 有 1 个 平 动 关节 和 两 个 旋转 关节 ， 并 且 保 证 能 够 到 达 每 个 点 。 





4) 在 三 维 笛 卡 儿 空间 中 ， 有 3 个 旋转 关节 ， 并 且 保 证 能 够 到 达 每 个 点 。 
K5-13 ”特定 机 械 手 设计 。 

使 用 工业 机 器 人 连 杆 设计 一 个 具有 3 自由 度 的 机 械 手 ， 以 便 满足 如 下 要 求 : 
D 当 两 个 关节 被 锁定 时 ， 机 械 手 的 末梢 点 绕 着 一 个 中 心 点 进行 圆周 运动 。 

2) 当 两 个 关节 被 锁定 时 ， 机 械 手 的 末梢 点 绕 着 全 局 坐标 系 原点 进行 周 周 运 动 。 
D 当 两 个 关节 被 锁定 时 ， 机 械 手 的 末梢 点 的 轨迹 为 直线 。 

4) 机 械 手 的 末梢 点 的 轨迹 是 一 条 通过 全 局 坐标 系 原 点 的 直线 。 

5-14 坐标 系 分 配 。 

图 5-58 描述 了 一 个 具有 4 自由 度 的 平面 机 械 手 。 
































图 5-58 4 自由 度 平面 机 械 手 


1) 按照 D-H 规则 分 配 连 杆 坐标 系 。 

2) 确定 机 械 手 的 连 杆 关节 表 。 

3) 确定 D-H 变换 矩阵。 

4) 确定 点 忆 的 坐标 ， 并 作为 关节 坐标 的 函数 。 

5) ÆA P 处 建立 一 坐标 系 ， 求解 正 向 运动 中 坐标 系 的 方位 。 
6) 确定 机 械 手 的 项 止 状态 及 变换 矩阵 。 
5-15 模块 化 的 关节 机 械 手 。 
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大 部 分 工业 机 器 人 都 是 模块 化 的 。 一 些 是 由 一 个 2 自由 度 机 械 手 再 附加 上 一 个 R FER 
(一 90 ) 机 械 臂 所 构成 的 。 关 节 式 机 械 手 是 由 一 个 如 图 5-52 所 示 的 2R 平面 机 械 手 附加 上 一 
个 如 图 5-51a 所 示 的 R 上 FR( 一 90") 机 械 辟 所 构成 的 。2R 平面 机 械 手 再 加 上 一 个 R ER 
(一 90°) 机 械 臂 便 可 形成 一 个 关节 式 机 械 手 。 将 所 要 求 的 变化 置 人 习题 5-3 和 习题 5-4 的 坐标 
系 中 ， 便 可 求 得 关节 式 机 械 手 中 连 杆 的 变换 矩阵 ， 从 而 确定 机 械 手 的 平衡 位 置 。 

5-16 ”建立 坐标 系 。 

图 5-59 所 示 为 某 3 自由 度 机 械 手 。 

1) 按照 D-H 规则 建立 连 杆 坐标 系 。 

2) 确定 机 械 手 的 连 杆 关节 表 。 

3) 确定 D-H 变换 矩阵 。 

4) 确定 作为 关节 坐标 函数 的 点 了 的 坐标 。 

D 在 点 P 建立 一 个 坐标 系 ， 求 解 正 向 运动 
确定 坐标 系 的 方位 。 

6) 确定 机 械 手 的 平衡 状态 和 变换 矩阵 。 

5-17 关节 式 机 械 手 。 

图 5-60 所 示 为 一 个 关节 式 机 械 手 。 

1) 按照 D-H 规则 建立 连 杆 坐标 系 。 图 5-59 3 日 由 度 

2) 确定 机 械 手 的 连 杆 关节 表 。 

3) 确定 D-H 变换 和 矩阵。 





















































KE 
车 
= 

1\ 

















Al 5-60 关节 式 机 械 手 





4) 确定 作为 关节 坐标 函数 的 点 卫 的 坐标 。 

D 在 点 建立 一 个 坐标 系 ， 求 解 正 向 运动 确定 坐标 系 的 方位 。 

6) 确定 机 械 手 的 平衡 状态 和 变换 矩阵 。 

5-18 模块 化 球形 机 械 手 。 

球形 机 械 手 是 由 一 个 如 图 5-56 所 示 的 极 坐标 机 械 手 附加 上 一 个 如 图 5-51b 所 示 的 R FR 
(一 90”) 机 械 辟 所 构成 的 。 极 坐标 机 械 手 再 加 上 一 个 R 上 FR( 一 90") 机 械 臂 便 可 形成 一 个 球形 
机 械 手 。 将 所 要 求 的 变化 置信 习题 5-3 和 习题 5-9 的 坐标 系 中 ， 便 可 求 得 球形 机 械 手 中 连 杆 
的 变换 矩阵 ， 进 而 确定 机 械 手 的 平衡 位 置 。 
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妈 5-19 非 工 业 连 杆 和 D-H 参数 。 
针对 图 5-61 所 示 的 一 组 非 工 业 连 杆 ， 建 立 D-H 坐标 系 并 分 派 D-H BR. 





连 杆 (i+1) 


图 5-61 一 组 非 工 业 连 杆 


5-20 ”模块 化 圆柱 形 机 器 人 。 

圆柱 形 机 械 手 是 由 一 个 如 图 5-57 所 示 的 2 自由 度 
笛 卡 儿 机 械 手 附加 上 一 个 如 图 5-51a 所 示 的 R FR 
(一 90) 机 械 臂 所 构成 的 。2 自由 度 笛 卡 儿 机 械 手 再 加 
上 一 个 RFR( 一 90") 机 械 辟 便 可 形成 一 个 圆柱 形 机 械 
辟 。 将 所 要 求 的 变化 置 入 习题 5-3 和 习题 5-11 的 坐标 
系 中 ， 便 可 求 得 圆柱 形 机 械 手 中 连 杆 的 变换 矩阵 ， 进 
而 确定 机 械 手 的 平衡 位 置 。 

5-21 PREV AY BRIG F bi 

球形 手腕 有 3 个 旋转 关节 ， 在 这 样 的 工作 方式 中 
它们 的 关节 轴 交 叉 于 一 个 共同 点 ， 该 点 被 称 为 手腕 点 。 
手腕 的 每 一 个 旋转 关节 附带 连接 2 根 连 杆 。 球 形 手 腕 
PREV FFU 5-62 所 示 。 对 图 5-62a、 图 5-62b 和 图 
5-62c 中 的 连 杆 ， 定 义 所 要 求 的 D-H 固定 坐标 系 。 试 求 
图 5-62a 所 示 结 构 所 对 应 的 变换 矩阵 3T, ， 5-62b 所 
示 结 构 所 对 应 的 变换 矩阵 +T;， 图 5-62c 所 示 结 构 所 对 
应 的 变换 矩阵 5Te 。 

妈 5-22 ”装配 的 球形 手腕 。 

依据 习题 5-21 中 所 建立 的 坐标 系 ， 对 图 5-63 中 给 
出 的 球形 手腕 标注 坐标 系 。 确 定 该 手腕 的 变换 算 d 
ESTs ATs 。 图 5-62 ”球形 手腕 的 拆 印 连 杆 

W5-23 5 自由 度 机 器 人 。 
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5-64 所 示 为 一 个 具有 球形 手腕 的 5 自由 度 机 器 人 。 














图 5-64 具有 球形 手腕 的 5 自由 度 机 器 人 














1) 按照 D-H 规则 建立 连 杆 坐标 系 ， 使 得 平衡 位 置 处 的 机 器 人 手 与 其 前 臂 呈 直线 。 

2) 确定 机 械 手 的 连 杆 关节 表 。 

3) 确定 D-H ARF, 

4) 确定 正 向 运动 的 最 终 变 换 矩 阵 。 

5) 确定 机 械 手 的 平衡 状态 和 变换 矩阵 。 

5-24 ”关节 式 机 器 人 。 

将 习题 5-22 中 的 球形 手腕 附加 到 习题 5-15 中 的 关节 式 机 器 人 ， 形 成 一 个 6 自由 度 机 器 
人 。 相 应 地 改变 习题 中 的 D-H 坐标 系 ， 求 解 机 器 人 的 正 向 运动 学 问题 。 

5-25 球形 机 器 人 。 

将 习题 5-22 中 的 球形 手腕 附加 到 习题 5-18 中 的 球形 机 械 手 ， 形 成 一 个 6 自由 度 球形 机 
器 人 。 相 应 地 改变 习题 中 的 D-H 坐标 系 ， 求 解 机 器 人 的 正 向 运动 学 问题 。 

5-26 圆柱 形 机 器 人 。 

将 习题 5-22 中 的 球形 手腕 附加 到 习题 5-20 中 的 圆柱 形 机 械 手 ， 形 成 一 个 6 自由 度 圆 柱 
形 机 器 人 。 相 应 地 改变 习题 中 的 D-H 坐标 系 ， 求 解 机 器 人 的 正 向 运动 学 问题 。 

5-27 ”RP 机械 手 。 
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5-65 所 示 为 一 个 RP 机械手， 要 求 该 机 械 手 的 末端 执行 器 可 以 沿 着 一 条 直线 滑动 
同时 围绕 着 相同 的 直线 旋转 。 请 给 机 械 手 中 的 连 杆 标注 坐标 系 ， 并 确定 末端 执行 器 相对 于 基 
体 的 变换 矩阵 。 

5-28 Eh 2R 机 械 手 。 

5-66 给 出 了 在 水 平面 内 动作 的 2R 平面 机 械 手 。 请 标注 坐标 系 ， 并 确定 末端 执行 器 在 
基体 坐标 系 中 的 变换 矩阵 。 














图 5-65 2 自由 度 R|P 机械手 图 5-66 EPI 2R 机 械 手 














5-29 SCARA 机 械 手 。 

将 2 自由 度 RP 机械手 附加 到 一 个 2R 平面 机 械 手 ， 可 得 到 一 个 SCARA 机 器 人 。 将 
习题 5-27 中 的 2 AHER P 机械手 附加 到 习题 5-28 中 的 2R 平面 机 械 手 中 ， 得 到 一 个 
SCARA 机 器 人 。 求 解 该 机 械 手 的 正 向 运动 学 问题 。 

We5-30 ”翻滚 -俯仰 - 偏 航 球形 手腕 运动 。 

将 所 要 求 的 D-H 坐标 系 附 加 到 图 5-30 中 的 翻滚 -俯仰 - 偏 航 球形 手腕 中 ， 类 似 于 网 5-28, 
确定 该 手腕 的 正 向 运动 。 

We5-31 俯仰 - 偏 航 -翻滚 球形 手腕 运动 。 

将 所 要 求 的 D-H 坐标 系 附加 到 图 5-31 中 的 翻滚 -俯仰 - 偏 航 球形 手腕 中 ， 类 似 于 网 5-28, 
确定 该 手腕 的 正 向 运动 。 

交 5-32 ”将 一 个 R-P-Y 手腕 装配 成 一 个 球形 手臂 。 

将 图 5-30 中 的 R-P-Y 球形 手腕 装配 成 图 5-40 中 的 球形 机 械 手 ， 确 定 该 机 器 人 的 正 向 
运动 。 

妈 5-33 ”将 一 个 P-Y-R 手腕 装配 成 一 个 球形 手臂 。 

将 图 5-31 中 的 P-Y-R 球形 手腕 装配 成 图 5-40 中 的 球形 机 械 手 ， 确 定 该 机 器 人 的 正 向 
运动 。 

妈 5-34 HARRE Bi SCARA 机 器 人 

将 习题 5-22 中 的 球形 手腕 附加 到 习题 5-29 中 的 SCARA 机 械 手 上 ， 形 成 了 一 个 7 自由 
度 机 器 人 。 相 应 地 改变 习题 中 的 D-H 坐标 系 ， 求 解 该 机 器 人 的 正 向 运动 问题 。 

We5-35 通过 螺旋 运动 形成 的 模块 化 的 关节 式 机 械 手 。 

利用 螺旋 运动 求解 习题 5-15 。 
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妇 5-36 通过 螺旋 运动 形成 的 模块 化 的 球形 机 械 手 。 
利用 螺旋 运动 求解 习题 5-18。 
妈 5-37 通过 螺旋 运动 形成 的 模块 化 的 圆柱 形 机 械 手 。 
利用 螺旋 运动 求解 习题 5-20。 
妇 5-38 ”通过 螺旋 运动 形成 的 球形 手腕 运动 。 
动 求解 习题 5-22 。 
妈 5-39 通过 螺旋 运动 形成 的 模块 化 SCARA 机 械 手 。 

利用 螺旋 运动 求解 习题 5-27、 习 题 5-28 和 习题 5-29 。 

妇 5-40 空间 站 远程 机 械 手 系统 。 

将 一 个 球形 手腕 附加 到 SSRMS 中 ， 形 成 了 一 个 10 自由 度 机 器 人 。 通 过 和 矩 阵 和 螺旋 运 
动 方法 求解 该 机 器 人 的 正 向 运动 。 

W541 安装 在 空间 站 上 远程 遥控 机 械 手 上 的 照相 机 。 

假设 我 们 将 检查 照相 机 安装 到 空间 站 远程 遥控 机 械 手 系统 中 的 连 杆 (4) 上， 如 图 5-67 
所 示 。 

确定 变换 矩阵 58Ty7 ， 使 得 zs 轴 指 向 手 爪 坐标 系 Bz 中 的 原点 ，zs 轴 处 于 (£3, x4) 平 
面 内 并 且 垂 直 于 x4 轴 。 这 时 确定 变换 矩阵 "Ts Ts,’ Ts, Ts ATs 







































































图 5-67 将 照相 机 附加 到 连 杆 〈4) 上 的 航天 飞机 远程 遥控 机 械 手 系统 (SSRMS) 
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对 于 给 定 配制 的 机 器 人 来 说 ， 什 么 是 关节 变量 呢 ? 这 就 是 逆向 运动 学 研究 的 问题 。 确 定 
关节 变量 能 够 简化 求解 一 组 非 线 性 耦合 
代数 方程 。 对 于 求解 逆向 运动 学 问题 而 
虽然 没有 标准 方法 和 常规 的 应 用 方 

， 但 仍 有 一 些 求 解 该 问题 的 分 析 方 法 
en 道 向 运动 学 的 主要 困难 是 
多 解 ， 例 如 图 6-1 所 示 的 平面 2R 机 
械 手 。 
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图 6-1 平面 2R 机 械 手 道 向 运动 学 问题 的 多 解 





依据 末端 执行 器 的 位 置 和 方向 确定 关节 变量 ， 称 之 为 逆向 运动 学 。 数 学 上 ， 道 向 运动 学 
主要 寻找 矢量 gq 中 的 元 素 。 


gg 一 (dl q2 q3 aa (6-1) 
当 变 换 和 矩阵 "T， 作 为 关节 变量 al q2, qas tt. dn 的 函数 给 定时 ， 有 
OT, =° Tı (qi)! T2(q2)?T3 Cq)’ Ta (qa) 1T, (gn) (6-2) 





TE SRL HE il FY BIL aie A Gd A ERDE EE Ti] HBR ON, YR TTT PE Jr TA BLA os RHP 
BERENS S.C. PE BL ASEH, BA tir Se Ty 28 Tia] I FB JL 2 Fa) ta iz of A 
为 了 控制 末端 执行 器 到 达 一 个 物体 的 配置 ， 必 须 求解 逆向 运动 。 因 此 ， 我 们 需要 知道 在 期 望 
的 方向 上 到 达 期 望 点 所 需 的 关节 变量 值 是 什么 。 

6 自由 度 机 器 人 的 正 向 运动 结果 是 一 个 4X4 的 变换 和 矩阵: 


ril r12 r13 ri4 























OTe =° Ti T2 T3 TtT T= 0 7 "2 (6-3) 
r31 r32 r33 Y 34 
0 0 0 1 
这 里 ，12 ee 然而 ， 因 为 左上 3X3 子 和 矩阵 是 旋转 
和 矩阵， 其 中 只 有 3 个 元 素 是 独立 的 ， 这 是 因为 满足 式 (2-197) 的 正 交 条 件 。 因 此 ， 式 (6-3) 
中 的 方程 中 只 有 6 个 方程 是 独立 的 。 
三 角 函 数 本 来 就 可 以 提供 多 个 解 ， 因 此 对 于 未 知 关 节 变 量 ， 当 求解 6 个 方程 时 ， 期 望 机 


(CA) 











器 人 有 多 种 配置 。 

有 可 能 将 逆向 运动 学 问题 解 耦 成 两 个 子 问题 ， 即 众所周知 的 逆向 位 置 运动 学 问题 和 逆向 
方向 运动 学 问题 。 这 样 解 耦 的 一 个 实际 结果 就 是 将 这 个 问题 分 解 为 两 个 独立 的 问题 ， 每 个 问 
题 只 有 3 个 未 知 参数 。 按 照 解 耦 原理 ， 机 器 人 的 综合 变换 矩阵 可 以 分 解 为 一 个 平 动 和 一 个 


转动 。 
ORG ode I dg ORG 0 
"Ts = =° D° R; = (6-4) 
0 1 0 1 0 1 


FE SHE ME? Dg 说 明了 末端 执行 器 在 坐标 系 Bo 中 的 位 置 ， 这 只 涉及 机 械 手 的 3 个 关节 变 
量 。 对 于 控制 手 肛 位 置 的 变量 ， 我 们 可 以 求解 "ds 。 转 动 矩阵 "Re BEAD T AR im ÍT ak LE AB 
系 Bo 中 的 方向 ， 这 也 只 涉及 手腕 的 3 个 关节 变量 。 对 于 控制 手腕 方向 的 变量 ,我 们 可 以 
求解 "Re 。 

证 明 : 

大 部 分 机 需 人 都 有 一 个 手 有 晓 ， 它 由 在 手腕 点 处 具有 正 交 轴 的 3 个 旋转 关节 所 构成 。 利 用 
球形 手 脏 ， 我 们 将 综合 正 向 运动 变换 矩阵 "Ts 分 解 成 手腕 方向 和 手腕 位 置 ， 从 而 解 看 手腕 和 























in| 














机 械 手 运动 学 。 
F aie 
oT; =° T; T; = (6-5) 
0 1 0 1 
手腕 方向 矩阵 为 
rll riz ri 
woavan r22 B (6-6) 
r31 732 733 
手腕 的 位 置 矢量 为 


d; = 





rT14 
| (6-7) 
r34 

手腕 矢量 %d6 =°ds. MGI S HLF et. Al. Oy SR AR DL a A BY) a It] ds a, 
我 们 必须 求解 "ds 以 确定 手腕 点 的 位 置 ， 然 后 求解 3Rs 以 确定 手腕 的 方向 。 

对 于 未 知 机 械 手 关节 变量 ,手腕 位 置 矢量 %4d6 ="Lv 提 供 3 个 方程 。 对 于 机 械 手 关节 变 
Ht, Rf de 可 以 使 用 式 (6-6) 计算 3Re6。 这 时 ， 对 于 手腕 关节 变量 ,可 以 求解 手腕 定向 
E 阵 3R6。 

假使 在 正 向 运动 学 中 包含 了 手 扑 坐标 系 ， 我 们 可 以 依据 下 列 方程 进行 分 解 ， 不 包括 来 自 
机 器 人 运动 学 中 手 爪 距离 dy 的 影响 。 

°T: =° Ti Ti =° T3377 6°T7 

















> 


0 
PR; dy)\(®Re OIL 0 moe 

0 1 0 | d7 

0 1 


在 这 种 情况 中 ， 首 向 运动 从 确定 %*Ts 开 始 ， 和 矩阵 "Ts 由 下 列 方程 求 得 : 
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oT, =° T,°T 7! 


1 
= T; 9 
0 
0 


例 6-1 关节 型 机 械 手 。 


考虑 如 图 6-2 所 示 的 关节 机 械 手 。 


(90")， 坐 标 系 间 的 变换 矩阵 为 





0 0 0 -1 
1 0 0 
一 0 
0 1 dr 
0 0 1 





1 0 0 0 

0 1 0 0 (6-9) 
0 0 1 ~d; 

0 0 0 1 


HLI FHETT ER ERO909, RI RCO) AR ER 











cosO1 0 sind; 
sind} 0 —cosé, 
oT = 
0 1 0 
0 0 0 
cos0» —sin@, 0 
sinf 一 cos0» 0 
IT, = 
0 0 i 
0 0 0 
cos; 0 sins 
5 sings 0 —cosé3 
2T; = 
0 I 0 
0 0 0 
机 械 手 的 正 向 运动 变换 矩阵 为 
coSsOl cos(@2 十 03 ) sind; 
sind; cos(@2 +03) —cosh; 
°T3=9T)1T22?T3= i 
sin(02 +03) 0 
0 0 





图 6-2 R 上 FRR 关节 机 械 手 
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0 
0 
(6-10) 
ly 
1 
12cos@2 
Zosin@» 
(6-11) 
0 
1 
0 
0 
(6-12) 
0 
1 


cos01sin(0, +8; ) 
sin0l sin(@2 +03) 
—cos(02 十 03 ) 











Lı cos cosO， 

Lı sinfı cos@2 

lı +15 sin» 
1 


(6-13) 


第 6 章 ”逆向 运动 学 





因此 ,末端 点 了 的 位 置 拓 量 为 














di 0 Z3sin(02 十 03 )cosO1 +12cos01 cosd2 
°dp=|dy |=°T3|0 | 一 | ssin(02 十 03 ) Sin01 +12 sind; cosd2 (6-14) 
dz L3 lı —l3cos(02 +63) +L2sinĝ2 


假设 点 P YER ENE Fi. A dp EFA UR BR. ESE AN Se MHE 
响 。 对 于 机 械 手 的 3 SIS AEE 1. 02, 03 dpe EET 3 个 方程 。 由 下 列 方程 可 求 得 01: 














dsin0l —d y cosh 一 0 (6-15) 
即 ， 有 
6, =arctan(d,/d,) (6-16) 
组 合 "dp 中 的 第 2 个 元 素 和 第 3 个 元 素 ， 可 以 发 现 : 
dcos0, +d y sinfı =13 sin(@2 +63) +l2cosd2 (6-17) 
现在 ， 组 合 "dp 的 第 2 个 方程 和 第 3 个 方程 ， 可 得 : 
(dz—lı —lzsin@z)? + (dzcos0; +d, sind; —l2cos62)? =13 (6-18) 
或 者 
—2lo(d,cos0; Fd y sinfı )cos0 +212 (lı —d z ) sinf? 
=13—((d,cos0; +d, sind)? +1? —2lid: +14 +d?) (6-19) 
这 是 式 (6-88) 所 示 的 三 角 方 程 形 式 。 
acos02 +b sinf: =c (6-20) 
a=—2l2(d,cos0; Fd ysin) 
b=2l:(lı—dz) (6-21) 





c=1f—[(d cosh, +d sinf01)? +1? —21,d,+13+d?] 
通过 该 方程 求解 0 。 式 (6-17) 调整 变形 后 ， 除 以 "dp 中 的 第 3 个 元 素 ， 可 得 Os: 
azcosOl 十 dvsin0l — L2 cosh? 
tan(O2 +03) li a re eee ae (6-22) 
d „cosi +d ysinO; —l2cosd2) | 
lı +l2sin02—d- 





02 (6-23) 





03 = stan? 


例 6-2 关节 型 机 械 手 的 几 种 情况 。 

为 了 检 量 例 6-1 中 的 北向 运动 学 方程 ,我们 检验 具有 下 列 尺 寸 的 关节 型 机 械 手 : 
lı=lm; 1 一 1.05m; /3 一 0.89m。 (6-24) 

当 末 端点 的 位 置 矢量 为 





odp=(1 1.1 1.2)T (6-25) 
由 式 (6-16) 可 得 01: 
Li 

0, Satan? (dy, d,)=arctan =0: 83298rad=47. 727° (6-26) 

为 了 求 取 0。， 我 们 求解 式 (6-20) 
acos +b sinf =c (6-27) 

这 里 

a=—212(d,cos0, td sinfı )=— 3. 941263019 (6-28) 
b=2l: (lı —dz) =—0. 5302360813 (6-29) 
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c =12—[(d,cos6, +d, siné,)? +1? 一 2l1d: 十 12 十 qd2] (6-30) 
=— 3. 232420149 或 c 一 一 5. 232420149 
对 于 c= 二 一 3. 232， 有 两 个 值 : 
02 =0. 7555416816rad~43. 28934959° (6-31) 
02 = — 0. 4880785028rad= — 27. 96483827° (6-32) 








对 于 c 王 一 5. 232, JEKI. 0s 由 式 (6-23) 求 得 。 如 果 O2=0.7555rad, WA 
d «cosi +d y sinfı — l2 cos2 
/1 十 /2Ssin0y >d: 

如 果 02 =— 0. 488rad, WA 
03 =— 0. 1913201914rad— 11° (6-34) 
例 6-3 2R 平面 机 械 手 的 逆向 运动 学 。 
依据 图 5-9 中 所 建立 的 坐标 系 ， 该 图 展示 了 具有 两 个 R || R 连 杆 的 2R 平面 机 械 手 ， 求 
得 该 机 械 手 的 正 向 运动 学 变换 矩阵 为 
cos(CO1 +42) sin(@; +62) 0 Licos0l 十 12cos(O1 +02) 
sin(0ı +02)  cos(ðı +02) 0 sin01 十 02sin(CO1 +42) 





fa =atan( )—02=0. 191820191 4rad~11" (6-33) 















































T: =° Tı T = (6-35) 
0 0 
0 0 1 
平面 机 械 手 的 逆向 运动 学 通常 比较 容易 求 取 。 机 械 手 末端 点 的 绝对 位 置 坐标 是 : 
PE 
= (6-36) 
Y Zi sinfı +l2sin(@1 +02) 
因此 ， 有 
X? +Y? =1?+12+21112cosb2 (6-37) 
XY 
cos: = 21 Lo (6-38) 
R= XY i 
2 二 arccos 211s (6-39) 
然而 ， 我 们 通常 避免 使 用 arscin 和 arccos， 因 为 它们 不 够 精确 。 因 此 ， 我 们 使 用 下 列 半 
角 公 式 
we $a cao 
求 得 0， 
0 一 十 zatan2 Pe (6-41) 





士 正 负 号 是 由 于 平方 根 会 有 两 个 解 导 致 的 。 这 两 个 解 被 称 为 提 肘 〈elbow up) 和 垂 肘 
(elbow down) ， 分 别 如 网 6-3a 和 图 6-3b 所 示 。 
提 肘 配置 的 第 1 个 关节 变量 0; 由 式 (6-42) 求 得 : 











jae eee (6-42) 
1 atan xX Fatan (ee anaes 
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图 6-3 2R 平 面 机 械 手 的 两 个 可 能 配置 








a) 提 肘 b) H 








垂 肘 配置 的 第 1 个 关节 变量 0 由 式 (6-43) 求 得 : 





A (6-43) 
1 =atan X Hatan cod, 





0, 也 可 由 下 列 替 换 方程 求 得 : 
— Xlo sinf 十 Y(CL1 十 cosO， ) 
Ylə sinf +X (Lı +12 cos@2 ) 
在 大 部 分 时 间 里 ， 依 据 X 的 符号 通过 加 减 x 可 校正 0; 的 值 。 也 可 以 结合 式 (6-36) 而 
确定 一 个 有 关 01 的 三 角 方 程 : 
2XL cosĝı +2Yl; sin0ı =X? +Y? +1? —12 (6-45) 
使 用 下 列 方程 也 是 很 方便 的 : 





01 =atan2 (6-44) 


X? +Y? +i — 1? 
1, +l2cos62= (6-46) 
21, 
有 关 0 和 09。 两 组 不 同 的 解 对 应 着 机 械 手 的 提 肘 和 垂 肘 配置 。 
例 6-4 2R 平面 机 械 手 的 运动 。 
考虑 具有 下 列 尺 寸 的 2R 平面 机 械 手 : 2) 二 lm, ls 二 1m。 (6-47) 


į Y/m 


























a 


oN 


Za Too 
I Į 


已 一己 -一己 一 一 





ERS 











~ X/m 





= T05 0 ~ 05 | 1 
Al 6-4 沿 着 一 条 从 Pi 到 P 的 直线 移动 且 外 二 lm H =m 的 2R 平 面 机 械 手 
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其 末端 点 沿 着 一 条 直线 从 P; (1.2，1.5) 点 移动 到 P。( 一 1.2，1.5)。 利 月 


Hit [a] Js hy 





学 方程 式 (6-39) 和 式 〈6-42) ， 我 们 可 以 确定 出 在 该 路 径 任 何 一 点 处 的 机 械 手 配 
说 明了 在 Pi Al Pe 之 间 的 42 个 等 间距 点 处 的 机 械 手 。 假 设 末 端点 沿 着 基于 下 列 日 
直线 进行 移动 。 





X=1.2-t Y=1.5 0<1:<2.4 
角度 91 Al A. 的 变化 如 图 6-5 rm. 





fA REC) 














l 时 间 t 
图 6-5 图 6-4 中 2R 平 面 机 械 手 的 角度 0 AO, 的 变化 





























例 6-5 关节 机 器 人 的 逆向 运动 学 。 





=, 图 6-4 
寺 间 行为 的 


(6-48) 


关节 机 器 人 的 正 向 和 运动 学 如 图 6-6 所 示 ， 在 例 5-33 中 已 经 被 探讨 过 了 ， 其 中 也 求解 了 





基于 手 脏 和 机 械 臂 变换 矩阵 的 末端 执行 器 综合 变换 矩阵 。 
T; =T im Twist =? Ts T7 


式 (5-124) HIR T FF Ne AB He RAE Twiss IU (5-74) OR fE T WUW FA ME BE Tamo SR 









































而 ， 依 据 如 图 6-6 所 示 的 新 建 坐标 系 ， 我 们 可 以 有 一 个 具有 6 WEITEM Tz, a 位移 变换 
的 6R 机 器 人 ， 其 连 杆 关节 类 型 见 表 6-1。 
相应 的 矩阵 为 
cosO1 0 sind, 0 
sind} 0 —cos@; 0 
oT, = (6-49) 
0 1 0 0 
0 0 0 1 
表 6-1 6-6 所 示 机 构 的 连 杆 关节 类 型 
1 R FR(90°) 4 R FR(— 90°) 
2 R | Ro’) 5 R FR(90°) 
3 R FR(90°) 6 R || Ro’) 
cosO —sinOz 0 /2cosO2 
sind» cos» 0 lesinds 
IT: = (6-50) 
0 0 1 d2 
0 0 0 1 
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基 座 
图 6-6 6 自由 度 关节 机 械 手 
CcOSO3 Sin03 0 
， sinf; 0 一 cos03 0 
“下 3 一 
0 jl 0 0 
0 0 0 1 
cos, 0 — sind, 
sind 4 0 cos 4 
aT, = 
0 =] 0 
0 0 0 
cosds; 0 sinds 0 
sinds 0 一 cos0O5 0 
1T; 一 
0 il 0 0 
0 0 0 1 
cos0e —sindg 0 0 
sin06 cos@s 0 0 
°T; = 
0 1 1 0 
0 0 0 1 
1 0 0 0 
0 1 0 0 
67T， 一 
0 0 1 ds 
0 0 0 
在 基体 坐标 系 中 夹 持 器 的 变换 矩阵 为 


(6-51) 


(6-52) 


(6-53) 


(6-54) 


(6-55) 


(6-56) 
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这 里 
c0ıc(02 F03) ~~ sj ch0ıs(02 +03) Llzc0z2c0ı +d2s0\ 
s0ıc(02 +03) 一 c0 sO, s(@2+63) Llzc0z2s0ı—d2c0ı 
T; = (6-57) 
s(02 十 03 ) 0 —c(O2 十 03 ) L282 
0 0 0 1 
ch4c0sc0s —s04805 —c0ssh, —chichss0e cOisds 0 
cO5c06s04 十 c04s06 ccOe—cOssOas0e sOis0s 0 
3T; = (6-58) 
— chs s05 s5 s06 c05 L3 
0 0 0 1 
t11 = chile +63) (cO1cO5 c0 — 804806) —cO6 805 8(02 +43) ] 十 SO (cg4s06 +05 cO5 S04) 
(6-59) 
t21 =s01Lc(O2 +63) (cO4c05 cOg —S04806) —cO6 805 s02 +63) ]— chi (cO4806 +cO5 cO6 S04) 
(6-60) 
£31 =s(02 +03) Cc01c05 chs — 801806) +06 805c(02 +83) (6-61) 
ti2=cO1[s(05 8068(02 +63) — c02 +63) Cehe 804 +cO4c05 806) | +801 (cO4c05 —cO5 804806) 
(6-62) 
t22 =s0iLs(O5 805 s02 +03) — c02 +43 ) Cehe 804 +0405 806) ]+cO1 (— chiche chs 801806) 
(6-63) 
t32 = — s05 s06 c(02 +03) —s(O2 +63) (cOgs04 十 c04c05S06 ) (6-64) 
t13 =s01 804805 +c61[.c0s s(02 +03) +c01805c(O2 +63) | (6-65) 
t23 = — 0 804805 +3801 [c058(O2 +03) +c64805cCO2 +63) | (6-66) 
t33 =c04s05s(02 +03) —cOsc(O2 +03 ) (6-67) 
tig =d6 {801801805 +c01 LcO1 805 c02 +03) +cO5s(02 +03) ]}+ 
ls3cO1s(02 +03) +d2s01 +l2c61 c02 (6-68) 
t21 =d s {—c0ı 804805 +80, LcO4805 c(02 +03) +c058(02 +03) |} + 
13801 8(62 +03) —d2c01 +1286) cO2 (6-69) 











t34 =d6LcO4sdss(O2 +3) —cO5s(O2 +63) | +13 c(O2 +03) +12 802 (6-70) 
chı[las(02 +03) +12cb2 | 十 dss01 dz 
d =| sO, [l3s(O2 +03) +l2c092]—d2chı |=] dy (6-71) 
l3c(02 +03) +12 802 dz 
逆 运 动 学 的 解 从 手 腊 位 置 矢 量 4 开 始 ， 手 腕 位 置 矢 量 4d 是 dr =O 时 "T 的 Gu ta ta). 
理论 上 ， 对 于 3 个 关节 变量 9; 、0。 和 03 ， 我 们 必须 能 够 求解 式 (6-71)。 因 此 有 


d,sin0; —d y cosh 一 Ca (6-72) 


























6, =2atan2(d,+./d2 +d} —d} ,d2—dy) (6-73) 
R (6-73) “di tdi >d; 时 有 两 个 解 ， di td) =d} 时 有 一 个 解 ， 当 d2 td) <a} 
时 无 实 解 。 
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综合 矢量 d 的 前 两 个 坐标 ， 可 得 : 








1asin(02 十 03) 一 士 Vdz 十 dy 一 42 —lz cosh (6-74) 
这 时 ， 利 用 矢量 qd 的 第 3 个 坐标 可 以 求 得 : 
13=(t,/d? +d}, — d} —l2cos02)? + (dz —l2 sind. )? (6-75) 


重新 调整 式 (6-75) 可 得 : 














acos +b sinf: =c (6-76) 
a=2l: Jd”. +d? —d3 (6-77) 
b=2lod- (6-78) 
c=d?} +d? +d? a oe (6-79) 
式 (6-76) 有 两 个 解 : 
C c? 
ta oan £ ,+ fF] -aanze s> (6-80) 
r ro 
r? =a? +b? (6-81) 
Rind 的 坐标 平方 和 为 : 
d? +d? +d? =d} +1} +13 +2l2l3sin( 202 十 03 ) (6-82) 


Hist (6-82) 可 得 





— 202 (6-83) 





d? 
03 aresin{ : 


求 得 了 关节 变量 91 02 和 03， 这 意味 着 我 们 可 以 找到 在 空间 中 的 手腕 点 。 然 而 ， 因 为 
在 "Ts 和 3T6 中 的 关节 变量 是 独立 的 ， 因 此 我 们 应 该 通过 3Ts6 或 者 ?Rs 求 得 04、05 和 06， 从 
而 找到 末端 执行 器 的 定位 。 
coO4cb5c06 一 SO4SO6 一 SO4c06 一 cO4c05SO6 cO4SO5 S11 512 513 
SR =| s04c05c0g +cO4 86 cO4c05 — s4 c05 s06 sass |=| s21 s22 s23| (6-84) 


— sô; cl s05 s06 c5 531 532 S33 


检查 3R6 中 的 元 素 ， 可 以 求 得 角度 04 、05 和 06。 


O=atan2(s235 s13) (6-85) 
05=atan2(/s1ss2s 5 s33) (6-86) 
0s =atan2 (s32, 一 831) (6-87) 


k 6-6 Rf = fA KZT TE acosd+bsind=c 
三 角 函 数 方程 的 原型 为 

acosO 十 psin0 王 < (6-88) 
通过 引入 新 的 变量 ”> Ald. AY DOK A = A es BOT FE Ls (6-88) J. 
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a 一 rsin0 (6-89) 
b =r cos (6-90) 
r 一 Va2 十 02 (6-91) 
é=atan2(a,b) (6-92) 


用 新 变量 代替 式 (6-88) 中 的 参数 ， 则 有 





C 








sin($ +4) =—— (6-93) 
= 
ester =+ l=] (6-94) 
r 
因此 ， 式 (6-88) 的 解 为 
2 
Paana ©, + 1-5] -atan2ca 6) (6-95) 
r r? 
或 者 
O=atan2(c,+./r?—c? )—atan2(a.b) (6-96) 

















ALL. X FOB acosO +bsind=c, WR r? =a? +b? >c, WANA ABs WR re =c’, 
WAR. WR r<, WFC. 
作为 一 个 示例 ， 我 们 求解 下 列 方程 。 


1. 5cos@+ 2. 5sinf =2. 549 (6-97) 
H a=1.5 UK b=2.5, 我们 可 求 得 x Mg. 





r=ĘẸxa? +b? 二 V1. 52 +2. 52 二 2. 915475947 (6-98) 
$=atan2(a ,0) 一 0.5404195rad (6-99) 
因此 ， 有 
0 =atan2(c,+/r?—c? )—atan2(a,b) 


=atan2(2. 549,+,/2)—¢ (6-100) 
=0. 5235718477rad BK 1. 537181805rad 
230° HK 88. 07° 

Ww 6-7 PHM atan2(y, z) 的 含义 。 

在 机 器 人 计算 中 ， 特 别 是 在 求解 逆 运 动 学 问题 中 ， 我 们 需要 求解 基于 正弦 函数 和 余弦 也 
数 的 角度 。 然 而 ，arctan 并 不 能 显示 分 子 和 分 母 符 号 的 有 效 性 。 它 总 是 在 第 一 象限 或 者 第 四 
象限 内 表述 角度 。 为 了 克服 这 个 问题 ， 为 了 确定 在 正确 象限 中 的 关节 角度 ， 引 入 函数 atan2 
函数 ， 如 下 : 
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y 
sgnyarctan | 一 
u 


了 sgny (x =0.y40) 


atan2(y ,Xx ) 一 (6-101) 





sgny [x—arctan 2 ) (1 <0, y FO) 
x 





An— nsgnz (xF0,y =0) 





1 (x >0) 
sen(xz)=4 0 (x=0) (6-102) 
一 (z0) 
作为 一 个 示例 ， 在 4 个 象限 中 对 于 4 个 点 ， 我 们 比较 arctan 和 atan2 。 
x=l, y=1, WA arctan(1/1)=0. 785, atan2(1, 1)=0. 785; 
x=—1, y=1, JW arctan(1/—1)=—0. 785, atan2(1,—1)=2. 356; 
Z 一 一 1、y 王 一 1， 则 有 arctan(—1/—1)=0. 785, atan2(—1,—1) =—2. 356; 
x=1, y=—1, WJ arctan(—1/1)=— 0.785, atan2(—1,1)=—0. 785. 
在 本 书 中 ， 无 论 上 述 问 题 被 提 及 与 否 ， 无 论 arctan (y/z) 被 使 用 与 否 ， 都 必须 基于 
atan2(y, x) 进行 计算 。 
KI 6-8 反正 弦 和 反 余 弦 函 数 的 基本 特点 。 


对 于 式 : 
sing =a, cosl =b tang =c (6-103) 
其 常规 解 为 
g =arcsina =(—1)*arcsina kx (6-104) 
0 = arccosb = + arccosb + 2kx (6-105) 
y=arctanc =arctanc Fkn (c? A—1) (6-106) 


KB 6-9 ”常规 情况 下 的 逆向 运动 学 公式 。 
有 一 些 常 规 三 角 函 数 方程 有 规律 地 出 现在 逆向 运动 学 问题 之 中 。 下 面 说 明 最 常见 的 方程 














及 其 解 。 
1) 如 果 
sind=a, (6-107) 
则 有 两 个 解 : 9 和 一 0 。 
0 一 atan2 Z (6-108) 
RE 
2) 如 果 
cosO 一 0， (6-109) 
则 有 两 个 解 : 9 和 一 0 。 
ery eae a (6-110) 


3) 如 果 
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sinf =a cosh =b, 


则 有 一 个 解 : 


a 
6=atan2 be 
4) 如 果 
acosé+bsin@=0, 


则 有 两 个 解 : 9 和 x +0, 





0=atan2 2 或 0 一 atan2 m 
b 一 0 








5) 如 果 
acos0O 十 bsin0 一 c， 

则 有 一 个 解 : 

0 一 atan2 —atan2 ee Se 

b C 

6) 如 果 
Qcos0O 十 bsin0 一 C、 
acos0O 一 0Sin0 一 以 ， 

则 有 一 个 解 : 
a? +b? =c? +d? 
0 一 atan2 ge ke 
ad +bc 

7) 如 果 


sind sing =a ,cos@sing=6b, 


则 有 两 个 解 : 0 和 x 十 0。 





6=atan2 s 或 0 一 atan2 = 


8) WMR 
sindsing =a cosh sing =b cosp =c , 


则 有 两 个 解 9 Flo: 9 相当 于 pg，9 十 x 相当 于 一 g。 








0=atan2 2 或 0 一 atan2 = 
b —b 


g=atan2 i 或 g=atan2 





G 


6.2 逆 变 换 技 术 


假设 已 知 说 明 全 局 〈 绝 对 ) 位 置 的 变换 矩阵 "Ts 和 基体 坐标 系 Bo 中 6 A 








端 执 行 器 的 定位 。 而 且 ， 假 设 已 知 每 个 几何 变换 矩阵 "Ti Cai), Te (gz)、 








(qa) Ts (q5) Te (ge) 都 是 关节 变量 的 函数 。 
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(6-111) 


(6-112) 


(6-113) 


(6-114) 


(6-115) 


(6-116) 


(6-117) 
(6-118) 


(6-119) 


(6-120) 


(6-121) 


(6-122) 


(6-123) 


(6-124) 


(6-125) 


由 度 机 器 人 末 
TCs) TY 





根据 正 向 运动 学 ， 有 


OTs =° Ti Ts Ts TtT’ Te = (6-126) 


0 0 0 1 
对 于 未 知 的 关节 变量 ， 我 们 可 以 通过 下 列 方程 求解 逆向 运动 学 问题 。 




















6 一"T1 Ts (6-127) 
To= Ts "TI "TS (6-128) 
°T =°T; T} "TI Ts (6-129) 
‘Te=° Tr °T; T3 OTI OTe (6-130) 
STe =T; °T Ts Ts Ty OTe (6-131) 
I=*T; ‘Ts AT Ty Ts "TI Te (6-132) 
证 明 : 
变换 矩阵 "T, 的 两 边 乘 以 "Ti 1 ， 可 得 
OTI Te =° TI COT T2’ T3 T, T5 Te) =' T6 (6-133) 


ER: OT) 是 4X4 和 矩阵 "Ti AAEE, KEE, A AH BC E E 
HAETI. 
st (6-133) 左边 是 gi WRK. A, AAEE T, 中 的 元 素 是 零 、 常 数 或 者 是 gq;、 
gq3、94、95 Meas WPM. HMMA A BOC He HE T Bek AYR BOTT BB. OK fee LE ROT 
程 可 得 qio 
这 时 ， 给 式 (6-133) APH FEL i RET |, HAE 
上 (6-134) 
式 (6-134) 左边 是 gz 的 函数 ， 与 此 同时 右边 ?T4 和 矩阵 中 的 元 素 是 零 、 常 数 或 者 是 q3、 
qas qs 和 ge 的 郴 数 。 求 解 右边 具有 零 或 者 常数 元 素 的 相关 元 素 ， 可 以 得 到 用 于 求解 g 的 
按照 这 个 过 程 ， 分 别 利用 下 列 等 式 ， 可 以 求 得 关节 变量 g3、gq4、gs 和 gq6。 
ey ae TT T TOT S= T, (6-135) 
ST, eT. LiT; ri “HT =T] 7. LIT; tep OT T Ta Ti T T =T, 















































(6-136) 
AT TZ TZ SOT OT, =T; BT T] TS OTT CT T? T T, T TO= T, 
(6-137) 
STUIT TI T7 T7 OT T= THT OTI T7 T7 OTT OP TT; T, T; T,) =I 
(6-138) 


Bi RANA IY 2 AT DA KA BES (Pieper) 技术 。 
il 6-10 关节 机 械 手 及 其 几 种 情况 。 
考虑 如 图 6-7 所 示 的 关节 机 械 手 。 其 坐标 系 之 间 的 变换 矩阵 为 
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cosO1 0 sind, 0 
‘ sing; 0 一 cosOl 0 
T,= (6-139) 
0 1 0 li 
0 0 0 1 
cos0» 一 Sin0， 0 /cosO， 
i sinf2 cosl2 0 lesin@2 
T; = (6-140) 
0 0 1 0 
0 0 0 1 
(6-141) 
图 6-7 关节 机 械 手 
机 械 手 的 正 向 运动 矩阵 为 
c0ıc(02 +03) SO1 coOlS(0O2 十 03) /Ll2c01c0» 
4 1 2 SOlc(O， 十 03 ) 一 cO1 sı s(02 +63) L2 s01 c02 
°T; =°T 1 T; T; = (6-142) 
s(02 +63) 0 —cec(02 +03) 11 十 /2SO> 
0 0 0 1 
假设 点 P 是 连接 球形 手腕 的 点 ， 我 们 连接 点 P 处 的 takht 坐标 Bi. ESB; 的 距离 为 
lz, WA 
1 0 0 0 
0 1 0 0 
= (6-143) 
0 1 1 k 
0 0 0 1 





因此 ， 机 械 手 的 综合 正 向 运动 学 矩阵 为 
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第 6 章 逆向 运动 学 
cOlc(CO， +43) SO1 coO1S(CO2 十 03 ) las(02 +843 )cO, HL2c01 c02 
0 0 3 sO, c(@2 +63) 一 cO1 SO1S(CO2 十 03 ) l3s(0» 十 03)sO1 十 /2SOlcO， 
T,= T3;°T,= (6-144) 
s(02 +63) 0 —c(0» 十 03) Ly lac(0,» | 03) | Z2SO， 
0 0 0 1 
使 用 下 列 尺寸 
/1 一 1m、 /2 一 1.00m、 3 一 0.89m。 (6-145) 
当 末 端点 位 置 矢 量 为 
"de = 2 (6-146) 
正 向 运动 矩阵 简化 为 
cO1c(0;> 十 03 ) SO1 coO1S(C0O2 +83) 1 
SOlc(O， +63) 一 cO1 sıs(02 +43) 1.1 
9 = (6-147) 
s(02 +03) 0 —c(02 十 03) 1.2 
0 0 0 1 
sk (6-147) 两 边 同 乘 以 "Ti |. WA 
OTI OT, =°T Ol ie ie T) = T, (6-148) 
这 里 
cos sind 0 0 
PS Wy 人 0 l =l T 
sind; 一 cosO1 0 0 
0 0 0 1 
cos(02 +03) 0  sin(0z:+8;) cosO1 +1. 1sin01 
sin(02 +63) 0 —cos(0: +0) 0. 2 
= (6-149) 
0 1 0 sind; — 1. lcosf; 
L 0 0 0 1 
c( A, +63) 0 s(02 +63) I. 1c +1. 2s(02 十 03 ) 
ETE ee s(02 +03) 0 —c(0: +03) 1. 1s02—-1. 2c(O2+43) 
T,°T;°T,= (6-150) 
0 1 0 
0 0 0 


式 (6-148) 左边 的 最 后 一 列 仪 是 01 的 函数 ， 与 此 同时 右边 是 
(6-148) FOR rw4 的 等 式 ， 可 得 到 求解 91 的 方程 : 


8; =atan2(1. 1,1) =arctan(1. 1/1)=0. 8329812667rad™47. 72631098° 
FA 01 =0. 83298rad 替换 式 (6-149) 中 的 9; ， 可 得 一 个 在 最 后 列 具有 数值 的 矩阵 1T， 。 





IT 





sin@; — 1. 1cosO1 





cos(02 F03) 0  sin(ð 
sin(02 +03) 0 —cos( 
0 1 
0 0 


st (6-153) 两 边 均 乘 以 IT ， 则 有 


© fF oO 


=0 





2 +03) 1. 4866 
02 +03) 0. 2 
0 0 

0 1 


2 A Os 的 函数 。 列 出 式 


(6-151) 
(6-152) 





(6-153) 
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IT, T, ='T;' CTs TST )=’T, (6-154) 
这 里 
coOs0O» sinf 0 一 1.05 
psp, =T, = 一 Sin0> cost, 0 0 iT, 
0 0 1 0 
0 0 0 1 
cos63 0 sins; 1. 486cosf2 +0. 2sin0， 一 1. 05 
sing3 0 一 cos03 0. 2cos@2 一 1. 486Ssin0， 
一 à i a P (6-155) 
0 0 0 1 
cos; 0 sind 3 0. 89sin@3 
TT, = sind; E 0. Pa ede 


0 0 0 1 
利用 式 (6-154) 左边 元 素 ru 和 2 的 平方 和 ， 可 得 求解 9; 的 方程 : 
(1. 486cos@2 十 0. 2sin@z 一 1.05)2 十 (0. 2cosd2 — 1. 486 sind, )? = (0. 89sin03)2 十 (一 0. 89cos03)? 








(6-157) 
3. 941cos@2 +0. 53sinĝ: =5. 232 (6-158) 
st (6-158) 具有 如 下 解 : 
02 =0. 755551822l1rad™43. 28993061° (6-159) 
02 = — 0. 488090807 3rad™ — 27. 96554327° (6-160) 
有 了 0， 我 们 可 以 通过 式 (6-156) Ask (6-155) 计算 出 Os. 
人 C10 
如 果 0: =0. 755rad, A 
03 =atan2(—0. 19437) =—0. 19198rad— 11° (6-162) 
如 果 02 =— 0. 488rad, Æ 
03 =atan2(0. 19437) =0. 19198rad~11° (6-163) 


例 6-11 球形 机 械 手 的 逆向 运动 学 。 
如 图 6-8 所 示 的 球形 机 器 人 ， 其 变换 矩阵 为 





cO1 0 一 SO 0 cA, 0 s02 0 

SO1 0 cO1 0 i sO, 0 一 c0， 0 
Ti = t= 

0 一 ] 0 0 0 1 0 l2 

0 0 0 1 0 0 0 1 

dl 0 0 0 c04 0 一 SO4 0 

9 0 1 0 0 SO4 0 cO4 0 
‘T; > T= 

0 1 1 ds 0 =] 0 0 

0 0 0 1 0 0 0 1 
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cOs 0 SO5 0 cO6 一 SO6 0 0 
3 sôs 0 一 c05 0 - SO6 cO6 0 0 
T= ET (6-164) 
0 1 0 0 0 0 1 0 
0 0 0 0 0 0 1 

















图 6-8 ”由 球形 机 械 手 连接 到 球形 手腕 上 的 球形 机 融 人 








因此 ， 对 于 一 组 关节 变量 ,可 以 利用 矩阵 乘法 求 得 末端 执行 絮 的 位 置 和 方向 ， 它 可 以 求 





解 正 向 运动 学 问题 。 
Fil r12 
5 7 21 r22 
Te =T T T Ti Ts T= 
r31 32 
0 0 





HP, BREE T, 中 的 元 素 与 式 〈5-159) 中 和 矩阵 的 元 素 相 同 。 


式 (6-165) 两 边 均 乘 以 "Ti 1 ， 可 得 


cos sind; 0 Of7rll riz ris 

op=op 一 i 0 —1 Ollrzs ree r23 
一 Sin01 cosd; 0 Ol|r31 r32 733 

0 0 0 1 0 0 0 


fai =f 35 coOSO1 一 73iSinO01 


其 中 , z 王 1，2，3，4。 基 于 已 知 的 变换 矩阵， 可 以 求 得 





(6-165) 


(6-166) 
(6-167) 
(6-168) 


(6-169) 
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fu fiz fi fu 
fa fz fz fora 
fs fa fa3s fa 
0 0 0 1 
fi =— 2801805 Hch (— $802 805 +02 cO4cO05 ) 
f21 =— 882801806 +06 (c02805 十 SO cO4cO5 ) 
fı =O 806 +s04c05 chs 

fi2=— 02801005 一 SO06 (—s02805 + cO2c01c05 ) 
f22=— 802801005 —s06 (cO2805 +s02c04 05) 
f32 =c04c06 — 80405 806 

f13=s02c0s +0204 805 
f23=— C0205 +802 c04805 


'Ts ='T2° Ts TT Te = 











f33 =s04s05 
fis =d3 s02 
fa=— dch 
J 34 一 /2 


式 (6-170) 中 唯一 的 常数 元 素 是 f34 二 1s， 因此 有 
rsa4cos01—riasin01 =L? 
这 类 三 角 函 数 方程 经 常 出 现在 机 器 人 逆向 运动 学 之 中 ， 它 有 一 个 对 称 解 。 
假设 ri4=rcos¢ 
ro4=rsing 
这 里 


2 
r=NyTri4 十 724 


= rea 
$ aretan o 
因此 ， 式 (6-183) 可 变 为 
L 
-= sing cosh: —cosġ sinfı =sin($ —01) 
式 (6-188) 表明 : 


cos(¢—01) =+./1—C2/r)? 
Alok, st (6-183) 的 解 01 为 
L2 


ifn 


r24 
6, =arctan — — arctan 
ri4 








如 图 6-9 所 示 ， 负 号 一 相当 于 机 器 人 的 左肩 配置 。 正 号 十 则 相当 于 右 肩 配置 。 











sk (6-170) WIR ru 和 2 仅 是 关节 变量 0 MO. Wee: 


太一 dasin0 一 r14cosOl 十 r24Sin01 





fo =d cosh: 一 一 r34 


因此 ， 有 可 能 利用 上 式 求 得 2 : 
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C 


( 
( 
( 
C 
( 
( 


6-170) 


6-171) 
6-172) 
6-173) 
6-174) 
6-175) 
6-176) 


(6.177) 


C 


( 
( 
C 
( 


( 
( 


C 


( 


C 


( 


C 


( 
( 


6-178) 
6-179) 
6-180) 
6-181) 
6-182) 





6-183) 


6-184) 
6-185) 


6-186) 


6-187) 


6-188) 


6-189) 


6-190) 


6-191) 
6-192) 





cosO1 +r24 sind 
02 =arctan 全 SRL - (6-193) 
r34 


XE, 0 必须 由 式 (6-190) 所 得 值 代替 。 
接 下 来 ， 从 下 列 方程 中 求 得 第 三 关节 变量 ds 。 








Tt. TT (6-194) 
这 里 
COSO， sind» 0 0 
ee 0 0 1 一 /， 
T; =| . (6-195) 
sinds —cosO, 0 
0 0 0 1 





— sass tcOacOscOs 一 SO4c06 —cOacOssOs chass 0 
cO4s06 十 SO4c05 c06 ch, 0s 一 SO4c05s06 SO4s05 0 
一 SO5c06 sôs s6 ðs d3 
0 0 0 1 
(6-196) 图 6-9 球形 机 器 人 的 左肩 配置 
利用 式 (6-194) 两 边 矩 阵 中 的 元 素 可 知 : 元 素 G, O 可 用 来 求解 dz- 
d3=r34cos02 riacosO1sing 十 24Sin0l Sin02 (6-197) 
因为 式 (6-194) 中 没有 其 他 的 元 素 是 另 一 单一 变量 的 函数 ， 因 此 我 们 放 到 下 一 部 由 式 
(6-198) 估算 04 ， 因 为 2?T3 T3 OT OTe =° T, 不 能 提供 新 的 方程 。 























EN fe 1 Pe I (6-198) 
计算 式 (6-198) PTs, 以 及 左边 部 分 。 
coss coss ”一 cosO5 cosO6 sin0s 0 
cosOgsinds  —sinĝssinĝs —cosé; 0 
T; = , (6-199) 
sind COSO6 0 0 
0 0 0 1 
1 0 0 0 
,| |0 10 0 
“Ts = (6-200) 
0 0 1 一 ds 
0 0 0 1 
cos 4 sin8 4 0 0 
ya 0 0 —1 0 
Ts: = , (6-201) 
一 Sin04 cosĝ4 0 0 
0 0 0 1 





由 式 (6-201) 得 
811 812 813 Zu 
£21 S22 B23 B24 
STI PTs Ts OTI T= (6-202) 
£31 £32 £33 £34 
0 0 0 1 
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g1i=—r3is02c04 rz2i(cO1s04 +881 cO284) Fri: (—s801 864 Hc0i chch) (6-203) 
221 =d 304i —r3i c02 r11 c01 S02 —r21 801 S02 (6-204) 
gsi =r31 802804 r21 (c01 c04 — $01 c82804) Hrii(—s61 cOy — 8 cO2 sO04) (6-205) 

其 中 , i=1, 2, 3, 4, FS ôu RIN WD NS 函数 ， 即 
esl came (6-206) 

0 G4) 

因此 ， 我 们 可 以 通过 元 素 (3，3) 相等 求解 0 ， 通 过 元 素 C1, 3) 或 者 元 素 (2, 3) 
相等 求解 45 ， 通 过 元 素 G, D 或 者 元 素 (3，2) 相等 求解 46 。 从 元 素 (3，3) 开始 ， 有 
713 (— 801 C04 — CO, CO2804) +1293 (cO1cO04 — 801 C02 804) +133 802804 =0 (6-207) 


求 得 04 为 




















一 r13SO1 十 723cO 
64 =arctan a eee (6-208) 


cə (r13.c01 十 723 SO1 )—r33 s02 
基于 01 的 第 2 个 值 ，04 也 等 于 








oT —r i3 s1 十 23cO1 
H = Aai c02 (r13c0ı 十 ”23SO1 )—r33 s02 C00 
现在 使 用 元 素 (1，3) 和 元 素 (2, 3), A 


sin05 =r23 (cosO1sinO4 + sinfı cos02 cosO4 ) —r33sinfz cosh, 十 

















r~13( 一 Sin0l sin04 +cos0; cosO2 cosh, ) (6-210) 
— coss = —r 33 cos02 ~ r13 cosĝ1 sinf02 —r23 sind) Sin02 (6-211) 
以 求 得 05 : 
0; =arctan Sint (6-212) 
cosd5 
最 后 ，06 可 由 元 素 G, D 和 元 素 G, 2) 求 得 
Sin06 王 7r3l1Sin02 Sin04 十 r21(cosO1l +cos@4 一 Sin0lcos0Oy sin04 ) 十 
ji11( 一 Sin0lcos04 一 cosOl cos@2 sin04 ) (6-213) 
cos06 =1r32sin@s sin@s +122 (cosi cosh, — sinfı cos@2 sinf, ) + 
712( 一 Sin0l cos@4 —cos@; cos@2 sin, ) (6-214) 
06 =arctan 2 (6-215) 
例 6-12 BREH fA ZE RE FY Gt FE 
基于 欧 拉 角 的 全 局 旋转 矩阵 在 式 (2-107) 中 已 经 求 得 。 
“Ry =(A -4 Aro Azo) =Rz.pRxoRz.4 
cpcp—cOsgsp 一 cpcy 一 cgOspcW sso ru ri? 113 
二 | cpcW 十 cgcpsW 一 SPSW 十 cOspcwW 一 SOcp | 二 |721 r22 r23 (6-216) 
sO sw sOcy c0 ral T32 T33 
用 有 3. 左 乘 变换 矩阵 Rh ， 可 得 
cosp sing 0 rico trz sp rizcp tr22 SO r13 co TT 238 
—sing cose 0 “Ry = —rusgetracp 一 r12SPD 十 7r22cCD r13 SFr co 
0 0 1 731 r32 r33 
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cosy — sing 0 
=| cossin% cosdcosd — sind (6-217) 
sindsing sin@cosp  cosð 
使 得 式 (6-217) 中 两 边 的 元 素 C1, 3) 相等 ， 即 
713cCOSO 十 7r23Sinp 一 0 (6-218) 
可 得 
gy =atan2(r13.—1r23) (6-219) 
有 了 yp 值 就 可 帮助 我 们 通过 元 素 A, D 和 元 素 C1. 2) Rg 
cosy =r; coso Fr: sing (6-220) 
— sing =r12 coso Tr22 sing (6-221) 
因此 ， 有 
y=atan2 ee eee (6-222) 
接 下 来 ，Rzu 右 乘 变换 矩阵 GR ， 可 得 
cosy sing 0 rucy—risp rizcy Triisy ris 
SRp| —sind cosy 0ļ|=|racp— rzs% raebtrash r23 
0 0 1 ralcy—razsy racy trasy r33 
cose 一 cosOsinp — sin@sing 
二 | Sinp cosOcosp ”一 Sin0cosp (6-223) 
0 sind cos 
sk (6-223) 中 元 素 (3，1) 两 边 相 等 ， 列 写 方 程 ， 求 得 V 
r31 cos% — r3 sing =0 (6-224) 
y=atan2(r31 ,731) (6-225) 
最 后 ， 利 用 元 素 G, D 和 元 素 (3，3) 可 求 得 0 
sind =r32cosp+rai sing (6-226) 
cosl =r33 (6-227) 
Me eas A (6-228) 
133 
例 6-13 已 知 欧 拉 角 变 换 矩 阵 的 逆 和 矩阵 。 
假设 基于 欧 拉 角 的 全 局 旋转 矩阵 已 给 定 : 
Rs =(Azy Axo Azo) =Rz,¢Rx0Rz.» 
copcp—cOsgsp —cech—cOsecp sOsp 0.12683 —0. 78033 0.61237 
二 | cpcyW 十 cgcpsW 一 SPSW TcOspcwW 一 SOcp |=] 0.92678 —0. 12683 —0. 35355 
SOSW SOcy cO 0. 35355 0.61237 0. 70711 
(6-229) 
HRZ 左 乘 变换 矩阵 SR， 可 得 
cosp sing 0 0. 126cp 十 0.926sp ”一 0.780cp 一 1.126sp ”0.612cp 一 0.353sp 
—sing cosp 0 SR =| 一 0. 126sp+0. 926cp —0.780sp—1.126cp —0. 353sp—0. 612cp 
0 0 1l 0. 35355 0. 61237 0. 70711 
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cosy — sing 0 
=| cosOsinW cosdcos# —sind (6-230) 
sinOsing sindcosd — cosd 


使 式 (6-230) 中 两 边 的 元 素 1, 3) 相等 ， 即 

















0. 61237cosp 一 0. 35355sing =0 (6-231) 
可 得 
p 一 atan2 人 人 ese) =I. 0472rad=60° (6-232) 
有 了 ov 值 ， 我 们 通过 元 素 A, D 和 元 素 A, 2) 就 可 求 得 y: 
cosy=0.126cosp0. 926sing (6-233) 
一 Siny 一 一 0.78cosp 一 0.126sinp (6-234) 
因此 ， 有 
一 atan2 dae Tatan? = =O. 523rad=30° (6-235) 
虽然 从 元 素 (2, 3) MEER (3，3) 中 可 以 求 得 9, 但 也 可 以 Rzy A REMEE Rg 
以 便利 用 逆 变 换 技 术 。 
cosy sin 0 0. 126cyt0.78sy —0. 78cy F0. 126sy 0. 61237 
SRp| sind cosy 0|=/0. 926cp~+0.126sp —0.0126c~+0. 926sW —0. 35355 
0 0 1 0. 353cH—0. 612s% —O0. 612ceW+0. 353s% 0. 70711 
cose —cosdsing — sin@sing 
=l|sing  cosdcosp —sindcose (6-236) 
0 sind cos 


st (6-235) 中 元 素 (3. D 两 边 相 等 ， 列 写 方程 ， 求 得 y: 








0.35355cosy—0. 61237sing =0 (6-237) 

o 0. 35355) _ _ f 
Y atan2 人 2 a) 0. 523rad=30 (6-238) 

最 后 ， 利 用 元 素 (3, 2) 和 元 素 (3, 3) 可 求 得 0， 
sind =0. 61237cosw +0. 35355sinw (6-239) 
cos =0. 70711 (6-240) 
0. 70711 5 

0 一 atan2 0. 70711 0. 785rad=45 (6-241) 


KB 6-14 ”逆向 运动 学 和 非 标 D-H 坐标 系 。 
考虑 如 图 5-4 所 示 的 3 自由 度 平面 机 械 手 。 该 机 械 手 的 非 标 D-H 变换 矩阵 为 








cosO1 一 Sin01 0 0 

sind; cos), 0 0 
i= (6-242) 

0 0 1 0 

0 0 0 1 
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求解 逆向 运动 学 问题 是 一 个 数学 问题 ， 采 月 





cos0» 一 Sin0， 0 li 
i sinĝ2 cosĝ2 0 0 
T, = 
0 0 1 0 
0 0 0 1 
cos03 一 Sin03 0 lz 
sind 3 cosl 3 0 0 
27, = 
0 0 1 0 
0 0 0 1 
1 0 0 h 
, 010 0 
3T, = 
0 0 1 0 
0 0 0 1 








方法 。 为 了 求解 逆向 运动 学 和 矩阵， 我 们 从 计算 正 向 运动 变换 矩阵 "T 开始 。 


SFT fT = 








sinl 123 COSO123 0 Lisin01 二 72sin012 十 13Sin0123 
0 0 1 0 
0 0 0 1 








RAEE T, 发 现 


oT TI! = T, T; T; = 


式 (6-249) 表明 : 


b 








(6-243) 


(6-244) 


(6-245) 


没有 简化 连 杆 坐标 系 的 标准 或 者 非 标 准 D-H 


cos0123 —sinĝiz3 0 {licos01 十 1»cos012 十 /3cosO123 





= (6-246) 
ral 132 T33 34 
0 0 0 1 
在 这 里 ， 我 们 使 用 下 面 的 简化 标记 以 简化 方程 。 
DO 人 0 (6-247) 
0123 =atan2(rai sri) (6-248) 
ru riz 0 74 一 037r11 cO123 —s0123 0 lichi Hlc 
ra rz 0 72 一 3r2 | |si cOi23 0 0s01 十 /2SO12 
0 0 1 0 fo 0 1 
0 0 0 1 0 0 0 1 
(6-249) 
21 £27272 
Pa arccos FE (6-250) 
ĝi =atan2(f2f3—fifasfifatfofa) (6-251) 
fi=ri ları = cı (lc +11) — sı Cl2s02)=chı f3 一 SO1 f4 (6-252) 
f2=ru—lyra 一 SOCL2c02 十 1) 十 cO C2802) =sti fa +c fa (6-253) 
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最 后 确定 ,角度 0 为 





妆 6.3 ERER 








首 同 运动 学 问题 是 为 了 寻找 一 组 


r2 
OT, =T(q)=°T; (qi)! T2 (q2)? T; Cq)’ Ta (qa) 1T, (gq)= 


或 者 


rij — 


XE, n 表示 自由 度数 。 然 而 ， 式 (6-255) 的 12 个 方程 之 中 的 最 多 6 Om =6) 个 方程 


< Eh 
变量 dko 


是 独立 的 ， 可 用 来 求解 关节 
确 给 定 。 


很 多 方法 都 可 用 来 求解 式 (6-255) 中 的 
就 是 众所周知 的 牛顿 -拉夫 森 (Newton-Raphson) 法 。 





用 的 方程 
在 迭代 技术 中 ， 为 了 求解 有 关 变 





对 于 关节 变量 ， 我 们 从 一 个 初始 猜想 
利用 正 向 运动 学 ， 对 于 所 猜想 的 关节 





基于 正 向 运动 学 所 计算 的 配置 和 所 期 
它 必 须 被 最 小 化 。 


一 组 方程 的 一 阶 泰勒 展开 式 为 


03 =0123 —01 — 02 (6-254) 
非 线 性 代数 方程 的 解 qro 
rll riz? 713 ru 
21 r22 123 Ta 
r31 732. 733 T34 
0 0 0 l 
(6-255) 
pe CE AT (6-256) 








PRT (gq) 是 超越 的 ， 可 通过 基于 正 向 和 运动 学 分 析 明 


i=) 
Ho 


然而 ， 所 用 方法 一 般 来 说 是 迭代 的 。 最 常 





tg 的 运动 学 方程 : 











T(q)=0 (6-257) 
开始 。 
q * =q tõq (6-258) 
变量 ， 我 们 可 以 确定 末端 执行 器 坐标 系 的 配置 。 
T*=T(g*) (6-259) 





望 配 置 之 间 的 差 值 代表 着 一 个 误差 ,我们 称 之 为 残 差 ， 


假设 òg <<I， 则 允许 我 们 用 一 组 线性 方程 代替 式 (6-260), 


这 里 ，J 是 方程 组 的 雅 可 比 和 矩阵 。 


因此 ， 未 知 变量 4 为 








oT=T—T* (6-260) 
oT 

T=T(q *+dq)=T(q soa a +O q?) (6-261) 
sT=Jdq (6-262) 

OT; 
Jq ;= ) (6-263) 

9gj 
Sq =J T (6-264) 
q =q * +J 16T (6-265) 
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我 们 可 以 将 由 式 (6-265) 所 获得 的 值 作为 一 个 新 的 近似 值 ， 重 复 计算 并 求解 新 值 。 下 

面 的 迭代 方程 中 总 结 这 种 重复 方法 ， 以 便 收敛 于 变量 的 确切 值 。 
q CP =q d+ C eT H) (6-266) 

在 一 个 算法 中 建立 起 这 种 迭代 技术 主要 是 为 了 数值 运算 。 

逆向 运动 学 迭代 技术 如 下 : 

1) 设置 初始 数 i 二 0。 

2) 寻找 或 者 猜想 一 个 初始 估计 值 g9 © 。 

3) 计算 残 差 T SJ (gq qg 

MRT QOO 中 的 每 个 元 素 或 者 它 的 范 数 上 TC(q 中 ) 中 小 于 一 个 误差 ,|T(q Olle, 
则 停止 迭代 ，gqg O EWE. 

4) 计算 q (i+1) =q (i) +J! (q (i) ST (q D) 

5) 设置 或 更 新 ;一 :十 1， 并 且 返 回 到 3), 

基于 变量 等 效 建立 误差 。 














q ÍP —q È <e (6-267) 
或 者 基于 雅 可 比 和 矩阵 有 : 
J —I<e (6-268) 
友 例 6-15 2R 平面 机 械 手 的 逆向 运动 学 。 
在 例 6-3 中 ， 我 们 已 经 知道 2R 平面 机 械 手 的 末端 点 可 以 由 下 式 摘 述 : 
X) {licos@ 十 22cos(O1 +02) 
ee 


JAY 2 fie OL bk FP Bh Te i OA, WL OR AR Ai AS As, RIEL: 





(6-269) 


H (6-270) 
Sl 
X 
r=] (6-271) 
Y 

















因此 ,方程 的 雅 可 比 和 矩阵 为 
IX oxX 
IT; 90; 902 —li sinfı —lzsin(ĝı +02) 一 /2sin(0 +02) 
rao- -) Adee ee ts /2cos(CO1 十 02 ) | 








9Y 9Y 
901 202 








(6-272) 
FETT FE FE PE HY a SR PE OA 
= 1 一 /02cos(O1 +02) 一 02Sin(O1 +62) 
072s02 m +lzcos(0ı +02) Lisin0l 十 /2Sin(CO1 | 
因此 ， 建 立 式 (6-266) 所 对 应 的 迭代 计算 式 为 


ONGD OND X XD 
AD lel + | eam 
02 02 Y Y 


lı =l2=1 (6-275) 

















J? (6-273) 
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从 一 个 猜想 值 开始 ， 有 
01 (0) 7/3 
(Oo) — >= 
í (| fee 


(He = 3/2 = 
la) lsince/3)+sin(x/3+—2/3)J UJ Gaye} 


XF ie FY ARE PY FE JE PE Se SE i Be Wy 


因为 





因此 ， 有 


=F? tar n/3 上 —2,/3/3 0 = 
A» 02 —x/3 J3 1 1 一 V3 /2 





基于 迭代 技术 ， 我们 可 以 求 得 下 列 的 值 ， 并 且 在 一 些 迭 代 中 求 得 
第 1 次 迭代 : 
V3 
J= 
3 
= 1 


—1/2 
8T 一 
1 一 V3 /2 
AyD (1.6245 
ieee 
A» 一 1.7792 


nw ren 


0.934 0. 988 
| 
0. 15553 


oof i 
` Z 一 1. 582 


一 1/2 
(i 


上 


解 。 


| 


1. 6245 
—1. 7792 


| 


(6-276) 


(6-277) 


(6-278) 


(6-279) 


(6-280) 


(6-281) 


(6-282) 


(6-283) 


(6-284) 


(6-285) 


(6-286) 


(6-287) 





一 1 0.433X10-3 
J= (6-288) 
0. 988 0. 999 
0. 119X107! 
= (6-289) 
—0. 362X107? 
0113) (1. 570795886 
q® = = (6-290) 
A> —1. 570867014 
第 4 次 迭代 
一 1.000 0.0 
j= (6-291) 
0. 98850 1.0 
一 0. 4381076 
ôT = (6-292) 
0. 711X10~4 
011G3) (1. 570796329 
q®= = (6-293) 
A> —1. 570796329 





第 4 次 迭代 的 结果 g “9 已 足够 接近 确切 值 g 一 (x/2 一 x/2)7，。 
克 6.4 逆 问 运动 技术 比较 


w6.4.1 解 的 存在 性 和 唯一 性 


显然 ， 当 期 望 的 末端 执行 器 坐标 位 置 “4a ?超出 了 机 器 人 的 工作 范围 时 ， 机 器 人 关节 变量 
没有 任何 实 解 。 在 这 种 情况 下 ， 平 方 根 符号 将 使 综合 结果 为 负 。 而 且 ， 甚 至 当 末 端 执行 器 位 
od ;在 机 带 人 的 工作 范围 之 内 时 ， 在 没有 突破 关节 约束 以 及 没有 违背 1 个 或 者 两 个 关节 
制 情 况 下 一 些 末 端 执 行 器 的 定位 "4d ?是 达 不 到 的 。 因 此 ， 通 常 来 说 ， 逆 向 运动 学 问题 是 否 存 
在 解 取决 于 机 器 人 的 几何 配置 。 

正常 情况 即 当 关节 数量 为 6 时。 假设 没 有 多 余 的 自由 度 ， 机 器 人 末端 执行 器 的 配置 在 工 
作 空 间 范 围 之 内 ， 道 向 运动 学 的 解 有 无 数 多 个 。 为 了 到 达 同 一 末端 执行 器 空间 位 置 ， 不 同 的 
解 均 相 当 于 可 能 的 配置 。 

总 的 来 说 ， 当 机 器 人 逆向 运动 学 的 解 存 在 时 ， 其 解 不 是 唯一 的 。 出 现 多 解 ， 这 是 因为 机 
器 人 以 不 同 的 配置 可 以 到 达 工 作 空间 范围 内 的 同一 点 。 例 6-3 中 2R 机 械 手 的 提 肘 和 垂 肘 的 
配置 就 是 一 个 简单 的 例子 。 

解 的 多 样 性 取决 于 机 械 手 的 关节 数目 及 其 类 型 。 机 械 手 有 多 解 的 事实 可 能 会 带 来 问题 ， 
因为 系统 必须 能 够 符合 这 些 解 中 的 一 个 解 。 虽 然 作 出 取 侈 的 标准 可 以 变化 ， 但 是 非常 合理 的 
选择 即 是 选择 相对 于 当前 配置 最 近 的 解 。 

当 关 节 数 量 小 于 6 时 ， 如 果 在 任务 空间 中 同一 时 间 内 的 自由 度 减 小 ， 将 存在 无 解 的 情 
况 ， 如 相对 于 当前 方向 约束 夹 持 需 的 定向 。 

当 关 节 数 量 大 于 6 时 ， 系 统 结构 将 变 得 元 余 ， 存 在 着 无 限 多 个 解 ， 在 机 器 人 工作 空间 范 
围 内 到 达 同 一 末端 执行 器 配置 。 对 于 安装 在 受 较 多 约束 环境 中 系统 来 说 ， 机 器 人 结构 宛 余 是 
一 个 有 趣 的 特征 。 从 运动 学 的 角度 看 ， 困 难 在 于 如 何以 数学 形式 系统 地 阐述 环境 约束 ， 以 确 
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保 逆 向 运动 学 问题 的 解 是 唯一 的 。 
六 6.4.2 逆向 运动 技术 


可 以 用 很 多 种 方法 求解 机 器 人 的 逆向 运动 学 问题 ， 例 如 解 耘 、 逆 变换 、 迭 代 、 螺 旋 代 
数 、 双 重 和 矩阵 、 双 重 四 元 数 和 其 他 几何 技术 。 利 用 4X4 齐 次 变换 矩阵 的 解 看 合 逆 变换 技术 
会 遇 到 这 样 的 问题 ， 对 于 一 个 特别 的 配置 ， 解 不 能 清楚 地 说 明 怎 样 从 多 个 可 能 的 解 中 选择 合 
适 的 解 。 这 样 一 来 ， 这 些 技术 取决 于 工程 师 的 技巧 和 直觉 。 和 迭代 法 通常 要 求 大 量 的 计算 ， 而 
旦 它 并 不 能 保证 收敛 于 正确 解 。 机 器 人 几乎 不 可 能 接近 于 齐 次 有 旦 衰退 的 配置 。 和 迭代 求解 法 也 
缺乏 从 多 个 可 能 的 解 中 选择 最 合适 解 的 方法 。 
虽然 理论 分 析 上 不 可 能 求解 一 组 非 线性 三 角 函 数 方程 ， 但 是 有 些 机 器 人 结构 是 有 分 析 解 
的 。 有 解 的 充分 条 件 是 : 当 6 自由 度 机 器 人 有 3 个 连续 的 旋转 关节 ， 并 且 其 旋转 关节 轴 交 叉 
于 一 点 。 逆 向 运动 学 的 其 他 特性 是 在 齐 次 点 中 解 的 不 确定 性 。 然 而 ， 当 求解 机 械 臂 方程 的 闭 
式 解 时 ， 这 些 解 很 少 是 唯一 的 。 

KI 6-16 ”迭代 技术 和 n、m 关系 。 

D 4n=m 时 的 迭代 法 。 当 关节 变量 数目 等 于 描述 正 向 运动 学 的 独立 方程 的 数量 
时 ， 这 时 假设 雅 可 比 和 矩阵 是 非 奇 异 的 ， 下 列 线性 化 方程 有 一 组 唯一 解 。 

5T 一 J5g (6-294) 
因此 ， 可 以 利用 牛顿 -拉夫 森 技 术 (Newton-Raphson technique) 求解 逆向 运动 学 问题 。 

该 过 程 的 成 本 取决 于 执行 迭代 的 次 数 ， 和 迭代 的 次 数 取 决 于 不 同 的 参数 ， 如 估计 解 和 有 效 
解 之 间 的 距离 、 解 处 的 雅 可 比 和 矩阵 条 件数 (制约 数 ) 等 。 因 为 逆向 运动 学 问题 的 解 不 是 唯一 
的 ， 依 据 估计 解 的 选择 可 以 产生 不 同 的 配置 。 如 果 解 的 初始 估计 超出 了 算法 的 收敛 范围 ， 可 
以 观察 到 其 非 收 敛 性 。 

2) n>m 时 的 迭代 法 。 

当 关 节 变 量 的 数 日 n 大 于 独立 方程 的 数量 mm 时 ， 就 是 一 个 超 定 情况 。 该 情况 通常 无 解 ， 
因为 关节 数量 不 足以 产生 末端 执行 器 的 一 个 任意 配置 。 但 可 以 产生 能 最 小 化 位 置 误差 的 一 
个 解 。 

3) 当 n 二 m 时 的 迭代 法 。 

当 关 节 变 量 的 数目 小 于 独立 方程 的 数量 m 时 ， 就 是 一 个 宛 余 情况 ， 该 情况 下 通常 
有 无 数 个 解 可 用 。 
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6.5 奇异 配置 


总 的 来 说 ， 对 于 任何 机 器 人 来 说 ， 苑 余 与 否 均 有 可 能 发 现 一 些 配 置 ， 称 之 为 奇异 配置 。 
在 奇异 配置 中 ,末端 执 行 器 自由 度 的 数目 相对 于 产生 动作 的 维 数 是 次 要 的 。 当 下 列 情况 发 生 
时 会 发 生 奇 异 配置 : 

1) 楼 柱 关节 的 两 个 轴 是 平行 的 。 

2) 旋转 关 方 的 两 个 轴 是 相同 的 。 

在 奇异 位 置 处 ， 末 端 执 行 器 失去 一 个 或 者 更 多 的 自由 度 ， 因 为 运动 方程 是 线性 相关 的 或 
者 确定 解 是 不 明确 的 。 随 着 移动 末端 执行 器 所 要 求 的 速率 理论 上 是 无 限 的 时 ， 必 须 避 人 免 奇 异 
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位 置 。 
由 雅 可 比 矩 阵 确定 奇异 配置 。 雅 可 比 矩 阵 J 使 得 末端 执行 右 的 无 限 微 小 位 移 与 无 限 微 
小 的 关节 变量 相关 。 

















ò X =[OX we OX n | (6-295) 
dq =[Ldg1, ee ,Sa ] (6-296) 

TETI EERE J 的 行列 数 是 mx Xn， 其 中 ERTA, m 是 末端 执行 器 的 自由 度数 。 
H n>m It, HEAT EIERE J WE, IFRA m—n 个 元 余 变量 。 


雅 可 比 矩 阵 J 也 确定 了 未 端 执行 器 速度 X 和 关节 速度 g 之 间 的 关系 。 


X=Jg (6-297) 
该 方程 作为 从 m ÆR EEEX Bl] 维和 撩 量 空 间 g 的 一 个 线性 映射 。 子 空间 RC) 
是 线性 映射 的 列 空间 ， 代 表 着 由 当前 配置 中 个 关节 所 产生 的 所 有 可 能 的 末端 执行 器 
WE. MET EEE J 是 一 个 满 行 和 铁 ， 这 意味 着 系统 在 那个 配置 中 不 会 现任 何 奇 异性 ， 
这 时 列 空间 RO) 涵盖 了 整个 矢量 空间 系 。 否 则 ， 至 少 存在 着 一 个 不 能 移动 末端 执行 器 的 
一 个 方向 。 
零 空间 NC) 代表 着 Jd =0 的 解 。 因 此 任何 矢量 9 ENJ) 并 不 会 产生 末端 执行 器 的 
任何 运动 。 
如 果 机 械 手 满 秩 ， 这 时 零 空 间 的 维 数 等 于 元 余 自 由 度数 闷 一 2。 当 雅 可 比 矩 阵 衰退 时 ， 
ROJ) 的 维 数 将 减少 ， 而 零 空 间 的 维 数 将 增加 了 相同 的 数量 。 因 此 有 
dimR(J) +dimN(J) =n (6-298) 
AE PT Ee RE J 不 再 满 秩 的 配置 相当 于 机 器 人 具有 奇异 性 ， 通 常 有 两 种 类 型 : 
1) 工作 空间 边界 奇异 性 CWork-space boundary singularities) 。 当 机 械 手 全 部 伸 出 或 自 
身 完 全 折 回 时 ， 便 会 出 工作 空间 边界 奇异 性 。 在 这 种 情况 中 ,末端 执行 器 接近 或 者 就 在 工作 
空间 边界 处 。 
2) 工作 空间 内 部 奇异 性 (Work-space interior singularities) 。 这 种 情况 出 现在 远离 边界 
处 。 在 这 种 情况 中 ， 通 和 常 有 两 个 或 者 更 多 轴 排 成 了 一 行 。 
在 数学 上 ， 通 过 计算 能 使 下 式 成 立 的 条 件 求解 奇异 性 配置 。 
|J | =0 (6-299) 

















































































































或 者 
| J? | =0 (6-300) 

在 机 器 人 学 中 ， 辨 别 和 避 开 奇异 性 配置 是 非常 重要 的 。 主 要 原因 如 下 : 

1) 运动 的 确定 方向 无 法 到 达 。 

2) 一 些 关 节 速 度 是 无 限 的 。 

3) 一 些 关 节 转 矩 是 无 限 的 。 

4) 逆向 运动 学 问题 不 存在 唯一 解 。 

通过 雅 可 比 和 矩阵 行列 式 可 以 检查 奇异 性 配置 ， 对 于 复杂 机 器 人 来 说 ， 这 是 一 个 乏味 的 任 
务 。 然 而 ， 对 于 有 球形 手腕 的 机 器 人 来 说 ， 则 有 可 能 将 奇异 性 检查 问题 分 解 为 如 下 两 个 独立 
的 问题 : 

1) 由 于 机 械 辟 运动 而 产生 的 机 械 臂 奇异 性 。 
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2) 由 于 手腕 运动 而 产生 的 手腕 奇异 性 。 


6.6 本 童 小结 








逆向 运动 学 是 指 对 于 给 定 的 末端 执行 器 坐标 系 的 位 置 和 方向 确定 机 絮 人 的 关 方 变量 。 一 
个 6 自由 度 机 器 人 的 正 向 运动 学 产生 一 个 下 面 的 4X4 变换 矩阵 。 


rıl 712 713 714 

0 0 1 2 3 4 5 °R "ds r21 22 723 724 
T; = T; T,°T; T, 下 了 一 = (6-301) 

0 1 r31 132 T33 134 





0 0 0 1 
XE, T, 中 的 12 个 元 素 中 只 有 6 个 元 素 是 独立 的 。 因 此 ， 对 于 给 定 的 "Te 和 矩阵， 逆向 
运动 学 问题 可 以 减少 到 求解 6 个 独立 元 素 。 
解 耦 技术 、 逆 向 变换 和 迭代 技术 是 求解 逆向 运动 学 问题 方法 中 的 3 个 已 被 应 用 的 方法 。 
在 解 耦 技术 里 ， 具 有 球形 手腕 的 机 器 人 逆向 运动 学 问题 可 以 被 分 解 成 两 个 子 问题 : 逆向 位 置 
运动 学 和 逆向 方向 运动 学 。 事 实 上 ,， 夹 持 需 变换 和 矩阵"T; 被 分 解 成 3 个子 和 拢 
ee Tes Te 和 5T， 。 









































TT TT (6-302) 

KE, EET, 定位 手腕 点 ， 取 决 于 3 个 机 械 手 关节 变量 ， PT, 是 手腕 变换 矩阵 ;*T， 
是 夹 持 器 变换 矩阵 。 

在 逆 问 变换 技术 中 ,我 们 逐步 地 从 下 面 的 和 矩阵 方程 中 抽取 仪 带 有 一 个 未 知 变量 的 

方程 。 

tT TT (6-303) 

2T, =1T71° TT T, (6-304) 

=T eT eT OT, (6-305) 

f= To fae Fa OT (6-306) 

Op =e TT Pe OT Pe (6-307) 














I Tee T] ?TF T7 OT) OT, (6-308) 
对 于 一 组 方程 T(g ) 王 0， 和 迭代 技术 是 寻求 关节 变量 矢量 4 的 一 种 数值 计算 方法 。 








习题 


6-1 标记 与 符号 。 
描述 下 列 这 些 符号 的 含义 。 
l)atan2(a.b) 2)"T，3)TCo) 4)g J 
6-2 3R 平 面 机 械 手 的 逆向 运动 
5-21 所 示 为 一 个 R1RIR 平 面 机 械 手 。 机 械 手 的 正 向 运动 形成 下 列 矩 阵 。 求 解 其 逆 
向 运动 ， 对 于 给 定 的 末端 点 坐标 (ro. yo) 和 给 定 的 角度 PP， 请 求 出 0 02 M0. 
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cos03 一 Sin03 0 /3s3cos0s 
> sins COSO3 0 /3sin03 
‘T, = 
` 0 0 I 0 
0 0 0 1 
cos0» 一 Sin0， 0 l2cosbs 
i sinf22 cosl2 0 /»sin0» 
T; = 
0 0 1 0 
0 0 0 1 
cos01 —sinĝı 0 {licos01 
sinfı cOSO1 0 /Lisin01 
T,= 
0 0 1 0 
0 0 0 1 








6-3 2R 机 械 手 末端 点 在 水 平方 向 上 的 路 径 。 

考虑 具有 /1 二 12 二 1 的 肘 形 平面 2R 机 械 手 。 末 端点 沿 着 从 点 Pid, 1.5) 到 Ps( 一 1， 
1.5) 的 直线 移动 。 

D 将 整个 笛 卡 儿 路 径 分 成 10 等 份 ， 确 定 在 11 个 点 处 的 关节 变量 。 

X2) 分 别 计算 Pi 和 Ps 处 的 关节 变量 01。 将 91 的 范围 分 成 10 等 份 ， 确 定 末端 点 在 
01 的 11 个 等 分 点 取 值 处 的 坐标 。 

X3) 分 别 计算 Pi 和 Ps 处 的 关节 变量 0 。 将 901 的 范围 分 成 10 等 份 ， 确 定 末端 点 在 
9 的 11 个 取 值 处 的 坐标 。 

6-4 ”倾斜 路 径 上 的 2R 机 械 手 末梢 点 。 

考虑 具有 /1 二 12 二 1 的 肘 形 平 面 2R 机 械 手 。 末 端点 沿 着 在 从 点 P1(1，1.5) 到 点 P? 
(一 1，1) 的 直线 上 移动 。 

D 将 整个 笛 卡 儿 路 径 分 成 10 等 份 ， 确 定 在 11 个 点 处 的 关节 变量 。 

X2) 计算 Pl 和 Ps 处 的 关节 变量 91。 将 01 的 范围 分 成 10 等 份 ， 确 定 末端 点 在 01 的 
11 个 取 值 处 的 坐标 。 

X3) 分 别 计算 Pi 和 Ps 处 的 关节 变量 0 。 将 91 的 范围 分 成 10 等 份 ， 确 定 末端 点 在 
9 的 11 个 取 值 处 的 坐标 。 

6-5 水 平 路 径 上 的 2R 机 械 手 运动 。 

考虑 具有 lL =lo=1 的 肘 形 平面 2R 机 械 手 。 依 据 下 列 的 时 间 函 数 ， 末 端点 沿 着 在 从 点 
Pi(1，1.5) 到 点 P:(—1, 1.5) 的 直线 上 移动 。 

X =1— 6t? +422 ,Y=1.5 

1) WR RAE S1, TPE AIS TA PRY 91 和 0: ， 并 绘制 其 曲线 。 

KD) 计算 作为 时 间 函 数 的 0 和 6 ， 并 绘制 其 曲线 。 

3) 计算 作为 时 间 函 数 的 FG, ， 并 绘制 其 曲线 。 

XO 计算 作为 时 间 函 数 的 人 AUG, ， 并 绘制 其 曲线 。 

6-6 平面 机 械 手 。 

6-10 所 示 为 一 个 3 自由 度 平 面 机 械 手 。 
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1) 确定 坐标 系 之 间 的 变换 矩阵 。 

2) 求解 正 向 运动 ， 对 于 一 组 给 定 的 关节 变量 1. d2, 03, ， 确 定 末 端 执行 器 坐标 系 Bs 
HRX, YUR o. 

3) 求解 道 向 运动 ， 对 于 一 组 给 定 的 末端 执行 器 坐标 匀 、Y 以 及 pg， 试 确定 关节 变量 01, 
das Oza 

6-7 水平 路 径 上 的 2R 机 械 手 运动 。 

考虑 具有 1) =lo=1 的 肘 形 平面 2R 
机 械 手 。 依 据 下 列 的 时 间 函 数 ， 末 端点 
以 定常 速度 沿 着 在 从 点 P1(1，1.5) 到 
点 Ps( 一 1，1.5) 的 直线 上 移动 。 

X=1—vt, Y=1.5 

1) 如 果 运 动 时 间 是 ON <1, HH 
速度 vu, FPA HO, 和 0。 曲线。 

2) 如 果 运 动 时 间 是 0 三 :三 5， 计算 
速度 v， 并 绘制 01 和 0， 曲线 。 

3) 如 果 运 动 时 间 是 0 委 : 科 10， 计 图 6-10 3 自由 度 平面 机 械 手 
AGRE v, FAH 91 AO. 曲线 。 

4) 在 点 (CO, 1.5) 处 绘制 作为 速度 v 函数 的 0 和 0， 曲线 。 

6-8 ”水平 路 径 上 的 2R 机 械 手 运动 。 

考虑 具有 lL) =lo=1 的 平面 肘 形 2R 机 械 手 。 依 据 下 列 的 时 间 函 数 ， 末 端点 以 定常 加 速 
度 沿 着 从 点 P1(1，1.5) 到 点 P;( 一 1，1.5) 的 直线 上 移动 。 




































































1 
X=1—at? Y= 


1) 如 果 运 动 时 间 满 足 0 三 :三 1， 计 算 加 速度 a， 并 绘制 901 AO, 曲线 。 

2) 如 果 运 动 时 间 满 足 0 二 :三 5， 计 算 加 速度 a， 并 绘制 91 和 0, 曲线。 

3) 如 果 运 动 时 间 满 足 0 三 :三 10， 计 算 加 速度 a， 并 绘制 91 和 0。 曲线 。 

K4) 在 点 (0，1.5) 处 绘制 作为 加 速度 函数 的 91 AO. 曲线 。 

6-9 ”倾斜 路 径 上 的 2R 机 械 手 运动 。 

考虑 具有 1) =l2=1 的 平面 肘 形 2R 机 械 手 。 依 据 下 列 的 时 间 函 数 ， 末 端点 以 定 值 速度 
沿 着 从 点 Pid, 1.5) 到 点 P:(—1, 1.5) 的 直线 上 移动 。 

和 X 一 1 一 过 ,了 一 1.5 

1) 如 果 运 动 时 间 满 足 0 二 + 二 1， 计算 并 绘制 91 和 0, 曲线 。 

2) 计算 并 绘制 作为 时 间 函 数 的 轴 和 09, 曲线 。 

3) 计算 并 绘制 作为 时 间 函 数 的 信和 如 曲线 。 

A) 计算 并 绘制 作为 时 间 函 数 的 AG, 曲线 。 

6-10 2R 平 面 机 械 手 中 可 接受 的 长 度 。 

2R 平面 机 械 手 的 末梢 点 ， 位 于 (1，1.1)。 

1) 假设 11 二 1。 对 于 提 肘 配置 绘制 91 和 0 对 2 的 曲线 并 且 确 定 可 能 72 的 范围 。 
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2) 假设 1s 二 1。 对 于 提 肘 配置 绘制 G9: AO. 对 2 的 曲线 并 且 确 定 可 能 2 的 范围 。 
3) 假设 01 =1。 对 于 垂 肘 配置 绘制 0 和 0。 对 2， 的 曲线 并 且 确 定 可 能 7， 的 范围 。 
4) 假设 1? 王 1。 对 于 垂 肘 配 置 绘制 02; AO. 对 721 的 曲线 并 且 确 定 可 能 21 的 范围 。 
6-11 直线 的 路 径 上 的 3R 机 械 手 末梢 点 。 
考虑 如 图 6-2 Pra AAA 1) =0.5, lo =13=1 的 3R 关节 式 机 械 手 。 末 端点 沿 着 在 从 点 
Pi(1.5，0，1) 到 点 P:(—1, 1, 1.5) 的 直线 上 移动 。 
D 将 整个 笛 卡 儿 路 径 分 成 10 等 份 ， 确 定 在 11 个 点 处 的 关节 变量 。 
2) 计算 在 Pi 和 P，* 处 的 关节 变量 0 。 将 01 的 范围 分 成 10 等 份 , 在 0 和 0s 的 11 个 
取 值 处 确定 末梢 点 的 坐标 。 
3) 计算 在 Pi M P: 处 的 关节 变量 9;。 将 9s 的 范围 分 成 10 等 份 , 在 91 和 0s 的 11 个 
取 值 处 确定 末梢 点 的 坐标 。 
4) 计算 在 Pi 和 Ps 处 的 关节 变量 93。 将 0s 的 范围 分 成 10 等 份 , 在 0 和 0; 的 11 个 
取 值 处 确定 末梢 点 的 坐标 。 
6-12 直线 的 路 径 上 的 3R 机 械 手 末梢 点 。 
考虑 如 图 6-2 所 示 的 具有 1 =0.5, lo =l3=1 的 3R 关节 式 机 械 手 。 依 据 下 列 时 间 函 
末端 点 沿 着 从 点 P1(1.5，0，1) 到 点 Ps( 一 1，1，1.5) 的 直线 上 移动 。 
X =1.5—0. 02523 +0. 0037544 —0. 0001525 
Y=0. 0123 一 0. 0015t4 +0. 0000625 
Z=1+0. 05¢3—0. 0007544 +0. 0000325 
1) 如 果 运 动 时 间 满 足 0 过 :上 委 1， 计 算 并 绘制 0) 02 MO; 曲线 。 
克 2) 计算 并 绘制 作为 时 间 函 数 的 抽 、0。 和 bs HHA 
交 3) 计算 并 绘制 作为 时 间 函 数 的 入 、9 和 入 曲线 。 
WA) 计算 并 绘制 作为 时 间 函 数 的 入 、 训 各 昌 线 。 
6-13 圆 形 路 径 上 的 提 肘 形 2R 机 械 手 。 
6-11 中 的 2R 机 械 手 有 /2 一 01 王 1。 假设 机 械 手 末梢 点 在 以 半径 尺 =1/3 的 圆 形 路 径 
上 运动 。 当 第 2 根 连 杆 处 于 水 平 状态 时 ， 假 设 机 械 手 开始 运动 。 
D WRAL Din et Tr eo, QA 6-lla 所 示 ， 绘制 0 AO. 曲线。 
2) 如 果 末 梢 点 以 顺 时 针 方 向 运动 ， 如 图 6-11b 所 示 ， 绘 制 0 和 0, 曲线 。 
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a) b) 
图 6-11 圆 形 路 径 上 的 提 肘 形 2R 机 械 手 
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6-14 3R 机 械 手 中 可 接受 的 长 度 。 
3R 关节 式 机 械 手 末梢 点 位 于 (1，1.1，0.5) 处 。 





























1) 假设 用 二 1 二 1， 绘制 O91、0， AMO; Wl. 的 曲线 ， 并 确定 1s 可 能 的 范围 。 
2) 假设 12 =13=1, 绘制 91，0s 和 03 对 2 的 曲线 ， 并 确定 2 可 能 的 范围 。 
3) (Ri 1, =lo=1, Bill 0i, 0: MO, Mls 的 曲线 ， 并 确定 2 可 能 的 范围 
¥ ia 
\ aA \ A 
| >y =X 
a) b) 


m 





图 6-12 (ABER EERE 2R BLA 





6-15 圆 形 路 径 上 的 垂 肘 形 2R 机 械 手 

图 6-12 中 的 2R 机 械 手 满足 2 三 0 三 1。 假 设 机 械 手 末梢 点 在 以 半径 尺 =17/3 的 圆 形 路 
径 上 运动 。 当 第 1 根 连 杆 处 于 水 平 状态 时 ,假设 机 械 手 开始 运动 。 

D 如 果 末 梢 点 以 道 时 针 方 向 运动 如 图 6-12a 所 示 ， 绘 制 91 和 0， 曲线 。 

2) 如 果 末 梢 点 以 顺 时 针 方 向 运动 如 图 6-12b 所 示 ， 绘 制 01 和 0, 曲线 。 

6-16 [AB RRA EW 2R 机 械 手 。 

图 6-13 中 的 2R 机 械 手 满足 L= 51, 假设 机 械 手 末梢 点 在 以 半径 R==4 并 且 圆 心 位 
于 Y 轴 上 的 圆 形 路 径 上 运动 。 






































图 6-13 [ABER EW 2R 机 械 手 


1) 假设 当 第 2 根 连 杆 处 于 水 平时 ， 提 肘 机 械 手 开始 在 上 部 的 圆 形 路 径 上 移动 。 绘 制 9 
和 Oo 直到 第 1 根 连 杆 在 路 径 末 端 处 于 水 平时 的 曲线 。 
2) 假设 当 第 1 根 连 杆 处 于 水 平时 ， 垂 肘 机 械 手 开始 在 上 部 的 圆 形 路 径 上 移动 。 绘 制 0 
Fl Oo 直到 第 1 根 连 杆 在 路 径 末 端 处 于 水 平时 的 曲线 。 
3) 假设 当 第 2 根 连 杆 处 于 水 平时 ， 提 肘 机 械 手 开始 在 下 部 的 圆 形 路 径 上 移动 。 绘 制 9 
和 Oo 直到 第 1 根 连 杆 在 路 径 末 端 处 于 水 平时 的 曲线 。 
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4) 假设 当 第 1 根 连 杆 处 于 水 平时 ， 垂 肘 机 械 手 开始 在 下 部 的 圆 形 路 径 上 移动 。 绘 制 01 
和 02 直到 第 1 根 连 杆 在 路 径 末 端 处 于 水 平时 的 曲线 。 

6-17 球形 手腕 逆向 运动 学 。 

5-26 所 示 为 一 个 具有 下 列 变换 矩阵 的 球形 手腕 。 假 设 坐标 系 Bs 是 基本 坐标 系 ， 求 解 
逆向 运动 ， 并 且 对 于 一 个 给 定 的 ?Te 求 04、05、06 。 























c4 0 一 SO4 0 cO5 0 sô; 0 chs — sls 0 0 
3 SO4 0 cO4 0 4 SO5 0 一 cO5 0 3 SO6 chs 0 0 
T,= T= T: = 

0 mal 0 0 0 1 0 0 0 0 1 0 

0 0 0 1 © 0 0 1 0 0 0 1 





妇 6-18 ” 翻 深 -俯仰 - 偏 航 球形 手腕 运动 。 

将 所 要 求 的 D-H 坐标 系 附加 到 图 5-30 中 的 翻滚 -俯仰 - 偏 航 球形 手腕 中 ， 类 似 于 图 5-28 
所 示 ， 确 定 手腕 的 正 向 运动 和 逆向 运动 。 

广 6-19 ”俯仰 - 偏 航 -翻滚 球形 手腕 运动 

将 所 要 求 的 D-H 坐标 系 附加 到 图 5-31 中 的 俯仰 - 偏 航 - 翻 深 球形 手腕 中 ， 类 似 于 图 5-28 
所 示 ， 确 定 手腕 的 正 向 运动 和 逆向 运动 。 

6-20 SCARA 机 器 人 逆向 运动 。 




















考虑 图 5-23 中 所 示 的 具有 下 列 变 换 和 矩阵 的 RR RP 机 器 人 。 求 解 逆向 运动 ， 并 且 
对 于 给 定 的 "*T,, R 0, 02, 0 Md, 
cosO1 —sind; 0 {icosOi cos0» 一 Sin0， 0  LcosO， 
6 sind cost 0 /isin01 i sin 2 cosO2 0 /2sin0; 
T, = 了 >， 一 
0 0 1 0 0 0 1 0 
0 0 0 下 0 0 0 1 
cos0O3 ”一 Sin03 0 0 1 0 0 0 
sind 3 cos03 0 0 3 0 1 0 0 
“T; = 3 T,= 
0 0 1 0 0 0 1 d 
0 0 0 1 0 0 0 1 
CO123 — s0123 0 lichi Tl»c01» 
ee ne ae sO0123  c0iz3 0 LSs0O1 十 /2S012 
T,=—°T,!T,?T;°T,= 
0 0 1 d 
0 0 0 1 


0123=01 十 0; 十 03 01; 二 01 十 0， 
6-21 R FR| R 关节 式 机 械 臂 逆向 运动 。 
图 5-22 给 出 了 3 AAR FRR 的 机 械 手 。 利 用 下 列 的 变换 矩阵 ,求解 91 、0，、03 
的 逆向 运动 。 








cos01 0 —sing1 0 cos0O —sing» 0 l2cos02 

or = sind, 0 cosd| 0 7, = sind» cosd» 0 lesinde 
0 =] 0 dı 7 0 0 1 d2 
0 0 0 1 0 0 0 1 
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COSO3 
Sin03 
0 
0 


3 


6-22 PRRR 机 械 手 的 运动 。 
PRRR 机 械 手 如 图 6-14 所 示 。 
1) 依据 标准 的 D-H 规则 ， 建 立 连 杆 坐 标 系 。 


0 
0 
1 
0 


Sin03 


一 COSO3 


0 
0 


2) 确定 每 根 连 杆 的 类 别 。 


3) 求 得 连 杆 的 变换 矩阵 。 
4) 计算 机 械 手 的 正 向 和 运动。 








0 
0 
0 
1 


5) 求解 机 械 手 的 OPA 


% 6-23 空间 站 远 


运动 。 


航天 飞机 远程 机 械 手 系统 (SSRMS) 在 原理 
上 如 图 5-24 所 示 。 机 器 人 正 问 运动 提供 
SE ME, RAR SSRMS 的 道 向 运动 。 
0 一 SO1 
0 c01 





cı 


°T, — 


tT; = 


T= 


0 


0 


oF O O 


0 
0 
1 
0 


遥控 机 械 手 系统 逆 问 


0 





提示 : 该 机 器 人 是 一 个 1 自由 度 元 
由 度 以 到 达 在 所 期 望 方向 的 任何 点 上 。 为 了 求解 这 个 机 器 人 的 逆向 运动 ， 我 们 需要 在 关节 变 
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t 下 列 变 换 








图 6-14 PRRR 机 械 手 


3T, 一 
0 0 1 d4 
0 0 0 1 
cA 0 — sls 0 
B sO ¢ 0 che 0 
9 Ts -一 
0 =A 0 ds 


余 机 器 人 。 它 有 7 个 关节 ， 有 着 多 于 所 要 求 的 6 个 自 





量 中 引入 一 个 附加 条 件 ， 
1) 假设 01 =0 并 |] 
2) 假设 9; 二 0 并 | 
3) 假设 0; =0 并 | 
4) 假设 0; =0 并 | 
5) 假设 05 =0 并 | 
6) 假设 97 =0 JF AL 


= 





四 io Mm fo 














或 者 给 关节 变量 中 的 一 个 变量 分 配 一 个 值 。 


工 7 
工 7 


工 7 


已 知 ， 试 确定 0 、03、04、05、06 和 07。 
EM, WHE, Os, Oas O5, 06 和 07。 
EM, WHE O, 02, Oas O5, 06 和 07。 
EM, WHE 01. 02, 03, O41, O6 和 07。 
EM, WHE 01. 02, 03, O14, 05 和 07。 
已 知 ， 试 确定 01. 02, 03, Oar 05 和 06。 

















7) WE Oi, O2, 03, Oa, Os, Os 和 07 以 使 了 最小。 





f =0ı +02 t03; +04 +05 t06 +07 
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在 全 局 坐标 系 G 中 ， 旋 转 刚 体 B 的 角速度 是 刚体 相对 于 G 的 瞬时 旋转 速度 。 角 速度 是 
一 个 矢量 ， 利 用 角速度 的 分 析 描 述 ， 我 们 引入 齐 次 变换 矩阵 的 速度 和 时 间 导 数 。 


7.1 角速度 矢量 及 其 矩阵 





考虑 在 参考 坐标 系 G(OXYZ) 中 具有 固定 点 O 的 
旋转 刚体 坐标 系 BCOxryz), WE 7-1 所 示 。 全 局 坐标 
系 和 刚体 坐标 系 之 间 的 时 间 变 化 旋转 变换 矩阵 可 以 表 
达 刚 体 的 运动 。 变 换 矩 阵 将 刚体 坐标 系 了 中 任何 固定 
点 的 瞬时 坐标 映射 到 全 局 坐标 G 中 的 相应 坐标 。 
































Gr G) = Rp)? r (7-1) 
R, Als 示 系 9 | 一 点 ii EY 
全 局 坐标 系 中 pine 的 速度 为 did pee eos ae 
Sy G)=° r SC RaG)r 有 固定 点 O 的 旋转 刚体 坐标 系 B(Ozyz) 
=¢ @ por (一 coBXSr t) (7-2) 


这 里 ,ow pe B 相对 于 G 的 角速度 矢量 ， 它 等 于 绕 着 一 个 瞬时 旋转 轴 a 的 旋 轴 速率 $。 


wl 
o =| w: |=¢a (7-3) 
w3 


角速度 矢量 与 斜 对 称 和 矩阵 G @ s 有 关 ,c @ nr 被 称 为 角速度 矩阵 。 





0 — w3 w2 
o =| ws 0 -| (7-4) 
—w? wl 0 
这 里 
co p=" Re RE=$u (7-5) 
iE AB : 


考虑 具有 固定 点 O 的 刚体 以 及 附加 的 坐标 系 B(Ozyz)， 如 图 7-2 所 示 。 刚 体 坐 标 系 最 
初 与 全 局 〈 绝 对 ) 坐标 系 G 重合 。 因 此 ， 刚 体 点 P HEREN 
(的 了 二 3 (7-6) 
位 置 矢量 sr 的 时 间 导 数 是 : 


CD 





G 


; d Gq Gd PE 
G =G =- G | B a G —G B i 
v r= G) jt Re(t)Br | ae Rg) r (to) J =FR gB) r (7-7) 


消除 式 (7-1) 和 式 (7-7) 之 间 的 Br ， 以 确定 点 











P 在 全 局 坐标 系 中 的 速度 。 
Gy =R BDS RTO) r G) (7-8) 
用 w 表示 矢量 Sr Q) WARM. A 
Cõr= RB R] (7-9) 
将 式 (7-8) BA 
Gy 一 GOBGT (t) (7-10) 
或 者 写 为 
Cy =c @ BX? r (t) (7-11) ”图 7-2 旋转 刚体 坐标 系 B 中 位 于 r 
正 交 条 件 SR sc 及 下 一 工 的 时 间 导数 引入 了 一 个 重 处 的 刚体 固定 点 P 
要 的 单位 矩阵。 
GRES REISRE RT=0 (7-12) 
利用 上 式 可 以 说 明 G Ø 8 二 [SReSRE) 是 一 个 斜 对 称 和 矩阵 ， 因 为 : 
SERBS RE=(C RBS RI) (7-13) 


矢量 G6w 被 称 为 从 全 局 坐标 系 G 来 看 刚体 B 相对 于 全 局 坐标 系 G 的 瞬时 角速度 。 
因为 任何 坐标 系 中 都 可 以 表述 矢量 方程 ， 因 此 可 以 利用 下 面 的 任何 一 个 表达 式 表述 刚体 
坐标 系 或 者 全 局 坐标 系 中 的 刚体 点 速度 。 


CV p=C@p_X orp (7-14) 








CU p=C@pX rp (7-15) 
这 里 ,&uwp 是 表述 在 全 局 坐标 系 中 点 P 的 全 局 (绝对 ) 速度 ;op 是 表述 在 刚体 坐标 系 也 
中 点 了 的 全 局 速度 。 


Sup ="Rpbvp = Rp (ho p X® rp) (7-16) 
利用 旋转 矩阵 ,Sw p ABD ,可 以 相互 转换 。 
B up =CRTE mp 一 GRTeOBGrp 一 GRTGRBGRTGrp 一 GRBGRPBErp (7-17) 
式 (7-17) 表明 : 
BB = RITER, (7-18) 


KEE CO, 被 称 为 从 全 局 坐标 系 B 来 看 刚体 B 相对 于 全 局 坐标 系 G 的 瞬时 角速度 矩阵 。 
从 cos Mios 的 定义 出 发 ,我们 能 够 通过 下 面 的 转换 得 到 两 个 角速度 矩阵 。 





GOB 一 GRBEOBSRE (7-19) 
&op 一 "REEOBSRB (7-20) 
或 者 等 效 于 : 
CRp=co@poRp (7-21) 
GRB 一 GRBROB (7-22) 
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GOp Rg = Rph 0B (7-23) 
如 果 G 和 B 在 同一 坐标 系 中 表述 的 话 ， 则 有 在 G 中 B 的 角速度 是 在 B 中 G 的 角速度 的 
负 值 。 
Sor =— [0G (7-24) 
Gøs =— oG (7-25) 
coB 总 是 以 下 列 形式 进行 表述 , 
Ee (7-26) 


KE, å ERT Foo 8 的 单位 矢量 ， 并 且 表 示 瞬 时 旋转 轴 。 
利用 罗 德 里 格 斯 旋转 公式 〈3-4)， 可 以 知道 : 




















Rag —$uRa,s (7-27) 
因此 ， 有 
=p ú (7-28) 
或 者 等 效 于 : 
G G 
c@rB =lim— R3, =lim——(—ad? cosg +using Hu? HI) =du (7-29) 
¿>o dt ¿>o dt 
因此 ， 有 
o =$ ú (7-30) 
例 7-1 刚体 点 绕 全 局 轴 的 旋转 。 


如 图 2-5 所 示 的 平板 绕 着 Z 轴 旋 转 ， 旋 转角 速度 a =10°/s. MERIR F a =30° AF, M 
APG, 30, 10) 的 全 局 〈 绝 对 ) 速度 为 


cosa —sina 0 5 —sina —cosa 0)\(5 
Sup ="Rp(t)®r P =~ sina cosa 0 30|=a| cosa —sina 0||30 
0 0 1 10 0 0 1)/\10 
一 Sin(r/6) —cos(r/6) 0)(5 —4.97 
= cos(Cr/6)  —sin(r/6) 0||30|=|—1.86 (7-31) 
0 0 1/\10 0 
这 时 , 点 也 的 位 置 矢 量 为 
cos(x/6) 一 Sin(Cr/6) O\(5 一 10. 67 
Grp =°Rp? rp=| sin(x/6) cos(r/6) 0||30|=| 28.48 (7-32) 
0 0 1) \10 10 
例 7-2 全 局 点 绕 着 全 局 轴 的 旋转 。 
如 图 2-5 所 示 的 木板 顶点 P 在 刚体 坐标 系 B 中 位 置 矢量 Srp 二 (5 30 ” 10) 。 当 木板 绕 
AZ 轴 旋 转 30" 时 ， 点 卫 的 全 局 位 置 矢量 为 
cos(z/6) —sin(x/6) O\(5 —10. 67 
Srp 一 “Rbarp 一 | sin(r/6) cos(r/6) 0||30|=| 28. 48 (7-33) 
0 0 1) \10 10 





如 果木 板 以 a =10°/s 的 角速度 旋转 ， 点 P 的 全 局 速度 将 为 
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一 Sin(Cr/6) 一 cosCr/6) 0)fcos(r/6) 一 Sin(Cr/6) 0 


Sup = "Ry RE rp = cos(m/6)  —sin(x/6) 0j||sin(Cr/6)  cos(r/6) 0 
0 0 1 0 0 1 
— 10. 67 — 4: 97 
28.48 |=| 一 1.86 (7-34) 


10 0 
例 7-3 主角 速度 。 
绕 着 X、Y、2 轴 的 主要 旋转 和 矩阵 为 
1 0 0 
Rx,y=|0 cosy —siny (7-35) 
0 siny cosy 
cosB 0 sinf 




















Ry,s = 0 1 0 (7-36) 
一 Sin8 0 cosB 
cosa —sina 0 
Rz. =| sina cosa 0 (7-37) 
0 0 1 
因 上 ， 它 们 的 时 间 导 数 分 别 为 
1 0 0 
Rx.y=Y 0 siny cosy (7-38) 
0 cosy —siny 
—sinB 0  cosß 
Rys=pB| 0 1 0 (7-39) 
—cos8 0 一 Sinp 
—sina 一 cosa 0 
Rza =à cosa —sina 0 (7-40) 
0 0 1 
因此 ， 绕 着 X、Y、2 的 主角 速度 矩阵 为 
0 0 0 
cõ@x=Rx R} ,=yl0 0 一 (7-41) 
0 1 0 
0 0 1 
c@y=Ry~R},=8| 0 0 0 (7-42) 
—-1 0 0 
0 一 1 0 
cõz=Rz RI ,=a|1 0 0 (7-43) 
0 0 0 
它们 等 效 于 
cõxy=y -I (7-44) 
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因此 ， 主 角速度 矢量 为 


c@z =wz ° K=a:K 























0 0 0 

Ba. =R! R, =—ġ|0 0 一 1| = 一 小 
0 1 0 
0 0 1 

Bo, =R] R, ¿=| 0 0 0|=—0.j 
—1 0 0 
0 一 1 0 

B: =RI R- p=—ọ|1 0 0|= 一 2 
0 0 0 


例 7-4 角速度 矢量 的 分 解 。 
每 个 角速度 矢量 都 能 被 分 解 成 3 个 主角 速度 矢量 。 
GOB 一 (GOB。 IDi+(cop s JÌ + cos : K)K 
=wxl+wyJ+wzK =yI+BJ+aK 
=O x twy THz 
Bl 7-5 角速度 矢量 的 合成 。 
从 一 个 旋转 的 合成 开始 








R ="R'!R> 
对 式 (7-54) 求 导 可 得 
0R,—°R 1R,+°R1!R> 
FAR (7-56) ~sh (7-58) 代替 旋转 的 导数 








R: =00:" R? 
Ri =0@1°Ri 
R.=1@2'R> 


则 有 


0@2°R2 =00@1°R1'!R2+°R11@2'R2=0@1°R2+°R1102°R IORIIR， 


fo ee 0 
= )0)°R2+7@2°R> 


$@2=Ri1@2°R{ 
因此 ， 可 得 


we aS g= 
00: =001 F102 


264 


(7-45) 
(7-46) 


(7-47) 


(7-48) 
(7-49) 


(7-50) 


(7-51) 


(7-52) 


(7-53) 


(7-54) 


(7-55) 


(7-56) 
(7-57) 
(7-58) 


(7-59) 


(7-60) 


(7-61) 









































s7% mee M 
TX (7-61) 说 明 角 速度 可 以 相对 地 加 在 一 起 。 
0@2= )@, + a (7-62) 
这 个 结果 也 适用 于 任何 数量 的 角速度 。 
0On=001 H102 +503 + +9, = >) je: (7-63) 
i=l 
il 7-6 用 欧 拉 角 频率 表示 的 角速度 。 
就 像 第 2 章 描述 的 那样 ， 欧 拉 角 频率 可 以 表述 角速度 矢量 。 因 此 ， 有 
8 wp =w,itwyj tok=ge,tbegthey 
sindsinw cosy 0 sindsind cosy 0 9 
=| sin0cosy +ô 一 siny | +ġ|0 |=| singcosw —sing 0 0 (7-64) 
cos0 0 1 cos0 0 1 į 
sindsing cosy 0 9 0 cosp sinĝsino 9 
6oas 一 BR5I8oB 一 BRGI|singcos 一 sinm 0||0 |=|0 sing 一 singcosp || 0 
CcOSO 0 1 ġ 1 0 COSO ġ 
(7-65) 
其 中 ， 欧 拉 变 换 矩 阵 的 逆 矩 阵 为 
cpcy—cOspsy 一 cpSVJ 一 cgOspcW sOsp 
BRG!=|ces¢+cdcosd 一 SpSW 十 cgOcpcw 一 SOcp (7-66) 
SOSW sco c0 


例 7-7 用 转动 频率 表示 的 角速度 。 
附录 A 和 附录 B 说 明了 12 个 全 局 三 























旋转 和 12 个 局 部 三 














轴 旋 转 。 利 用 这 些 方程 ， 我 


作为 一 个 例子 ， 考 虑 附录 











们 能 够 求 得 用 转动 频率 表示 的 刚体 角速度 矩阵 和 刚体 角速度 矢量 。 
B 中 情况 9 的 欧 拉 角 变换 矩阵 。 
BRG =RzyR2z oR z,¢ (7-67) 
角速度 矩阵 等 于 
。 。 Z, dR, oR z 
bBo =" RoPRE =R- oR a OR:,y dg Hy Re Re |X Regs Rep)" 
— dR - 9 T di di dRi, E Ti 4 T 
=OR.,4R 0 do R. oR oR. rd Re +p dé RT oR, R: (7-68) 
式 (7-68) AY FE MET shy 
0 cos0 —sindcosp 0 0 sind 0 1 0 
BOG =o] — cosh 0 sind sind +6 0 0 cosy |+ġ|—1 0 0 
sin0cosw —sindsind 0 —sing — cosg 0 0 0 0 
(7-69) 
或 者 
0 db +¢@ cosh 6 sing —¢ sinf cosy 
B® c= —ġ —ọ cosh 0 6 cosp+@ sindsing (7-70) 
6 sing +¢ sindcosy — Â cos —¢ sinf sing 0 
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相应 的 角速度 矢量 为 
6 cosp+¢ sind sing sindsing cosy 019 
BOG = —| ő sing +¢ sindcosy 二 一 | sin0gcosw —sing 0 0 
i Peco cosd 0 1 į 
BØG =— GOB 
oo = hon 
因此 ， 有 
sindsing cosy 07 
Bop=|singcosy —sinp 0 0 
cosd 0 1 d 


We 7-8 角速度 的 坐标 变换 。 


坐标 系 Bo 相对 于 坐标 系 Bi 并 且 在 





坐标 系 Bi 中 表示 的 角速度 io 可 


= _ ga 
R10: RT =0 0? 


中 表示 为 
为 了 说 明 这 个 方程 ， 将 式 (7-75) 
将 为 


Rao Rr =0 R110, 


两 边 同 乘 以 任意 矢量 "r 


"Ror ="Rii@e'r =R; Gø: X!r ) 


="Ri@2X°Ri r =°@: Xr 


st (7-75) 的 左边 等 于 右边 的 部 分 ， 


即 有 


102°r 一 4@o，X0r 


友 例 7-9 单位 矢量 的 时 间 导 数 。 





(7-71) 


(7-72) 
(7-73) 


(7-74) 


以 在 基体 坐标 系 Bo 


(7-75) 


o ALK, st (7-75) 的 左边 


(7-76) 


(7-77) 


应 用 式 (7-15)， 我 们 可 以 定义 刚体 坐标 系 B G. js h) 单位 矢量 在 全 局 坐标 系 G CI, 


了 ，KK) 中 旋转 的 时 间 导 数 。 





Gp Teme Xt 
Gq} 

J _B 
— ”一 ;XX 
ge COBA 
Cdk 

= 

一 ;XEk 
a. 


WG 7-10 四 元 数 和 欧 拉 参数 的 角 





欧 拉 参数 也 可 以 表述 角速度 矢量 。 
Gr =e(t)®re* 
这 里 
我 们 可 以 知道 


* 十 eBre 


cr =e Pre 
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速度 。 
从 一 个 有 限 旋 转 的 单元 四 元 数 表述 开始 。 


(1)=e(t)Bre !(t) 


e=eote 
e* =e 1 一 ec0 一 e 
“一 ee*cr 十 cree “一 2ece*cr 


(7-78) 


(7-79) 


(7-80) 


(7-81) 


(7-82) 
(7-83) 


(7-84) 








因此 ， 角 速度 四 元 数 为 



































G@p=2e e (7-85) 
利用 单位 四 元 数 的 正 交 特 性 ， 有 
ee! =ee* 一 1 (7-86) 
可 以 有 
e* tee * =0 (7-87) 
利用 四 元 数 之 积 可 以 扩展 角 速 een 而 得 到 基于 欧 拉 参 数 的 角速度 分 量 。 
G@p=2ee* 一 2(eo 十 ec)(eo 一 ce) 一 2(eoeo 十 eoe —evete *e—e Xe) 
0 bo ee] e 
ecei—e1eotezes—esez| |e, éo -és éz ||—e1 
= f . ;| 二 (7-88) 
e062 — ezeo — eies Heze e ea i a aTe 
eoe3 十 ele? 一 ezel 一 e3e0 be =é; i Ed ~ © 
角速度 四 元 数 的 标量 分 量 等 于 零 ， 因 为 
egeo te * e=eoeo tee) te2e2 tezes =0 (7-89) 
角速度 矢量 也 可 以 定义 为 一 个 四 元 数 
GoB 一 26 e* (7-90) 
利用 式 (7-90) 可 以 定义 转 旋 四 元 数 的 时 间 导 数 。 
é = Fou e (7-91) 
坐标 变换 可 以 将 角速度 变换 至 刚体 坐标 系 之 中 ， 即 
eo Ëi e? e3 eo 
Bo oe O e (7-92) 
—es —e3 eo el lle 
men ear ilh 
因此 ， 有 
Beon=2Ze (7-93) 
T Lg a 
2 00n (7-94) 
n 7-11. KAMERMAN. 
欧 拉 参数 的 旋转 矩阵 5Ra FETE (3-82) 中 已 给 定 。 
ez ter?—e3—e2 2(leies—eoes) 2(eoey 十 ele3) Pr me FE 
GRp=| 2(ecesteier) ei—eiXe3—es 2(ese3—eoe1) | 一 |r ree rz 
2(ele3 一 ecoe?) 2legei tezes) eg—ei— este; r31 T32 T33 
(7-95) 
从 式 (3-149) 至 式 〈3-152) 中 的 任何 一 组 中 可 以 求 得 每 个 参数 。 由 第 一 组 可 求 得 参数 : 
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| E i 1 r32 r2 Iru ra lirar 
<0 一 工 了 7 Fru Trz Trss e177 a 可 e=] 
(7-96) 
因此 ， 有 
= r 22 TT 33 (7-97) 
8eo 
el 一 er r23 eo — (r32 —1r 93 eo | (7-98) 
ed 
e: 一 ans ae r31 eo — (ris —rai eo] (7-99) 
e? 
e3 = i mer r12 eo — (rai —1rie deo | (7-100) 
ed 
利用 变换 矩阵 微分 
GRB 一 GOGBGRB 
可 以 得 到 
eo z (e101 erw e303) (7-101) 
j 1 
e= 2 (eowl1 十 eyow3 一 e3ywy ) (7-102) 
j 1 
e: = z Cow? e1w3 T ezw] ) (7-103) 
j 1 
e3 一 2 (eow3 +e1w2—e201) (7-104) 
类 似 地 ， 我 们 可 以 求 得 el 、e* 、es ， 而 且 可 以 将 最 终结 果 设 置 成 矩阵 的 形式 : 
eo 0 w] w? w3 \ feo 
el 1 | ol 0 w3 一 2 || ei 
=> (7-105) 
ez 2 lw? 一 os 0 wl es 
č; w3 w2 —w] 0 e3 
或 者 
eo eo es —e: —ei\/0 
el lje eo — ës e2 wl 
| => (7-106) 
e2 2 e2 el eo —e3 || w2 
3 e3 — ez el1 eo w3 
KG 7-12 角速度 矩阵 的 元 素 。 
利用 式 (3-144) 中 所 引入 的 排列 符号 
1 
E ijk zC JIG —RkRICk—ið t57 R=1,2,3 (7-107) 


ZK (7-107) 可 以 使 我 们 求 得 角速度 矩阵 @ 中 的 元 素 ， 当 角速度 矢量 w = (ol wz 
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w3)1 给 定时 。 


Wij =Eijkwe (7-108) 


太 7.2 时 间 导 数 和 坐标 系 


矢量 的 时 间 导 数 取 决 于 我 们 求 导 的 坐标 系 。 矢 量 r 在 全 局 坐标 系 中 的 时 间 导 数 称 为 G 导 
数 ， 标 记 为 


Gd 
ce 
与 此 同时 ， 矢 量 在 刚体 坐标 系 中 的 时 间 导 数 被 称 为 B 导数 ， 标 记 为 
Bd 
ae. 








导数 符号 左上 角 表 示 进 行 导数 所 在 的 坐标 系 ， 因 此 ， 单 位 矢量 可 以 作为 常量 考虑 。 
此 ,rp 在 坐标 系 BB 中 的 导数 和 在 坐标 系 G 中 的 导数 分 别 是 为 











g rea rem vr 5rityjtzk (7-109) 
Cd. = : ee eee 
qe rp 一 re=°ve=XI+YJ+ZK (7-110) 


为 了 求 得 ?rp 的 G 导数 和 rp 的 B 导数 ,我 们 可 以 将 式 (7-15) 用 于 表述 在 刚体 坐标 系 
中 刚体 固定 点 P 的 全 局 速度 ， 以 定义 混合 导数 。 刚 体 矢量 rp 的 G 导数 标记 为 








B db 

es (7-111) 
类 似 地 ， 全 局 矢量 Srp 的 B 导数 标记 为 

ds 

Bop=— orp (7-112) 


当 点 P 在 坐标 系 B 中 移动 时 ， 同 时 坐标 系 B 也 在 坐标 系 G 中 旋转 ， 刚 体 矢量 3rp 的 G 
Gd 


g re O= rerin rer (7-113) 
全 局 矢量 srp 的 B 导数 被 定义 为 

B 

本 rp) 一 rp 一 GOBXSrP 一 ErP (7-114) 


iE AA: 

假设 具有 单位 矢量 i、j、 太 的 G (OXYZ) 是 全 局 坐标 系 ， 具 有 单位 矢量 i、 了、 的 
B (Oxyz) 是 局 部 坐标 系 。 移动 点 了 的 位 置 和 撩 量 如 图 7-3 所 示 ， 在 刚体 坐标 系 和 全 局 坐标 
系 中 分 别 表述 为 














Brp@=x(Hity@j+tz@sk (7-115) 
S pp(@Q)=X@I+tY@J+Z@K (7-116) 
刚体 矢量 arp 在 坐标 系 B 中 的 时 间 导 数 和 全 局 矢量 crp 在 坐标 系 G 中 的 时 间 导 数 分 别 为 
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re rea vp Hz ity Oj tz GDR 
(7-117) 
cd 


Grp 一 Grp 一 Gup 一 入 (IT +Ý Nİ +Ż0@)K 





(7-118) 

因为 在 式 〈7-115) 中 坐标 系 B 的 单位 矢量 和 在 式 
(7-116) PARRA G 中 的 单位 矢量 作为 常数 考虑 。 

利用 式 〈7-111) 的 定义 ， 我 们 可 以 求 得 位 置 矢 



































Y 
sienna 图 7-3 在 旋转 刚体 坐标 系 B 中 处 
Srp =a i tyj+zk) 于 矢量 ar G) 处 的 移动 刚体 点 P 
pd Gdj ， Cdk 
TIT eet y C de dt 
=Brp +BøØ@g X (zi +yj +z É) 
=Brp +808 XPrp 
Bd 
= (7-119) 








获得 这 个 结果 因为 矢量 arp 的 分 量 z、y、>* 是 标量 。 标 量 相 对 于 坐标 变换 是 不 变 的 。 因 
此 ， 如 果 ae 是 一 个 标量 ， 则 有 





Sd Bd 











L= =2 (7-120) 
dt dt 
全 局 矢量 crp 的 B 导数 为 
= oes LY f+ZK) 
dt oe dt 
VD 六 Pal ， Bdj) BdK 
=XI+YJ+ZK+X T HY a: HZ J 
一 Grp 十 SaocXSrp (7-121) 
因此 ， 有 
Bd : 
gq rea rp cosrX? rp (7-122) 
坐标 系 B 相对 于 坐标 系 G 的 角速度 是 一 个 矢量 ， 可 以 在 如 下 任何 一 个 坐标 系 中 表示 。 
Gøs =wx Iltwy J +wz K (7-123) 
B øB =w: i+w; J Hw: k (7-124) 


例 7-13 坐标 系 B 绕 2Z 轴 的 旋转 。 
刚体 坐标 系 B 在 坐标 系 G 中 绕 着 Z 轴 旋 转 a 角 。 因 此 ， 位 于 Br 处 的 点 P 在 坐标 系 G 中 





的 位 置 拓 量 为 
cosa —sing 0)\(x xcosa— ysina 
crp 一 CRBBr SRz aa Pr =| sina cosa 0||y |=|xsina+ycosa (7-125) 
0 0 1)\z 之 


坐标 系 也 的 角速度 和 矩阵 为 
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G05B 一 GRBGRT 一 < 开 (7-126) 
于 是 有 
c@r =a K (7-127) 
我 们 可 以 求 得 6@ pW ARIAS 
BO p="REC@RoREB=a k (7-128) 
因此 ， 有 
Rap=ak (7-129) 
现在 ,我 们 可 以 求 得 下 列 导 数 : 
Bd : 
一 Br =®r =0 (7-130) 
dt 
cq. ad x cosa — y sina 
cP G 一 Sr = EP x sina + y cosa 
=(—2x a sina — y a cosa )Î+ (x a cosa — y a sina) +z K (7-131) 
对 于 混合 导数 ， 我 们 从 在 坐标 系 B 表述 的 全 局 速度 开始 。 
cd cit ee i eee 
am =BopX®r =a |0 |X| y l=al x |=yaitzaj=br (7-132) 
1 之 0 
我 们 可 以 将 Br 转换 至 全 局 坐标 系 之 中 ， 得 到 全 局 速度 表达 式 为 
cosa —sina 0)(—y —ycosa— r sina 
Gr 一 GRBBr =a | sina cosa 0|| x |=a | 一 ysinae 十 zcosa 
0 0 1 0 0 
=¢ (—ycosa—zxsina)I +a (x cosa — y sina) J (7-133) 
下 面 成 的 导数 就 是 相对 于 坐标 系 B 且 表 述 在 坐标 系 G 中 的 刚体 点 速度 。 
—ycosa—2z sina 0 x cosa — y sina 0 
i 一 GOBXGr =a | — ysina +x cosa |—a |0 |X| xsina +ycosa |=10 
0 1 之 0 


(7-134) 
Gl 7-14 在 坐标 系 B 中 移动 点 的 时 间 导 数 。 
考虑 局 部 坐标 系 B， 绕 着 Z 轴 以 a 旋转 ， 移 动 点 位 于 矢量 arp (1)==ti1 处 。 因 此 ， 有 


crp 一 5RBBrPp =Rz..(t)’rp 





cosa 一 Sina 0)\(t 
=| sina cosa 01]0]=tcosa+¢sinaJ (7-135) 
0 0 1) \0 
角速度 矩阵 为 
cB =SRrSRI =a K (7-136) 


则 有 
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c@B =a K (7-137) 
也 可 以 验证 
Br = CRIS ðB Rg =a k (7-138) 
因此 ， 有 
Bop=ak (7-139) 
现在 我 们 可 以 求 得 下 列 导数 : 
Bq 
ee a Big - 
FF rp rp= i (7-140) 
Gq. i ‘ r : R 
q re TIT p= Cosa ta sina ) I + (sina +ta cosa) J (7-141) 
对 于 混合 导数 ， 我 们 从 式 (7-142) 开始 。 
1 0 t 1 
Sd Bd s f z . 
a te ar +BeopXBrp=l|0/+al0]X]0l=lr¢ |=ittaj=Brp (7-142) 
0 1 0 0 





st (7-142) HE P 表述 在 坐标 系 B 中 的 全 局 速度 。 然 而 ， 我 们 可 以 将 速度 和 失 量 Brp 转 
换 至 全 局 坐标 系 ， 并 且 求 得 表述 在 坐标 系 G 中 的 全 局 速度 。 





cosa —sina 0f1 cosa —ta sina 
Crp =°Rp8rp =|sing cose Olga |=] sina + te cosa 
0 0 l/o 0 
= (cosa — ta sina) I+ (sina +t a cosa) J (7-143) 
下 一 个 导数 就 是 : 
ee, =Spp—S@, X Srp 
dt 
cosa — ta sina 0 tcosa cosa 
=| sina +ta cosa |—a |0 |X]|tsina |=| sine 
0 1 0 0 
=(cosa)i+(sina)f =€rp (7-144) 


sk (7-144) 就 是 点 已 相对 于 坐标 系 也 并 且 表 述 在 坐标 系 G 中 的 速度 。 为 了 表述 在 坐标 
系 了 中 的 速度 ， 我 们 应 用 坐标 系 变换 。 


cosa —sina 0\T/cosa 1 
BrP 一 GRISGrP 一 sina cosa 0 sina |=|0 |=: (7-145) 
0 0 1 0 0 





有 了 时， 如 果 我 们 将 矢量 转换 到 与 求 导 所 在 的 同一 坐标 系 之 中 ， 则 式 (7-145) 有 更 多 的 
应 用 ， 这 时 可 应 用 微分 算 子 。 因 此 ， 有 
、 tcosa cosa — ta sina 
dy ind G B = : = 。 
T re =y. Rpg rp) tsina | 一 | sing 十 ia cosa (7-146) 
0 0 
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t 1 

Bq Bq Bq 

We = “GE PIG =e = = 

g to E (CRporp) if (7-147) 
0 0 


例 7-15 ”位置 矢量 和 速度 矢量 的 正 交 性 。 
在 全 局 坐标 系 中 ， 如 果 一 刚体 点 的 位 置 矢量 由 符号 r 标记 ， 则 有 
dr 

















T er =0 (7-148) 
为 了 展示 这 个 特性 ， 我 们 可 以 对 式 〈7-149) 进行 求 导 。 
r'r =r? (7-149) 
则 有 
“or a ‘r-+r 7 =p er =0 (7-150) 




















Xt FE AG hn Ae ABE AK RE a, 式 (7-148) 都 是 正确 的 ， 只 要 矢量 和 导数 能 在 
同一 坐标 系 中 表述 即 可 。 

KB 7-16 导数 变换 公式 。 

在 刚体 坐标 系 B (Oryz) 中 ,一 固定 点 的 全 局 速度 可 由 式 (7-2) 求 得 。 现 在 考虑 能 在 
坐标 系 B 〈Ozyz) 中 移动 的 一 个 点 PP。 在 这 种 情况 下 ， 刚 体位 置 矢量 *rp 并 不 是 常量 ， 
此 表述 在 坐标 系 B (Oryze) 中 这 样 的 一 个 点 的 全 局 速度 为 


















































pp hee rer (7-151) 
di rp di rP GOB 下 及 一 全 天 
有 时 ， 利 用 式 〈7-151) 的 结果 可 以 定义 从 刚体 到 全 局 坐标 系 的 微分 算 子 变换 。 
cd _ Bd B 
ae = FE c@sB Xg =g (7-152) 




















然而 ， 必 须 注意 矢 量 口 和 所 表述 最 终结 果 所 在 的 坐标 系 。 最 终结 果 是 8 口 ， 它 表明 表述 在 刚 
体 坐 标 系 B (Oxyz) 中 的 全 局 时 间 导 数 。 矢 量 口 可 以 是 任何 矢量 ， 如 位 置 、 速 度 、 角 速度 、 
动量 、 角 速度 ， 甚 至 是 随时 间 变 化 的 力 矢量 。 

式 (7-152) 被 称 为 导数 变换 公式 ， 它 与 矢量 的 时 间 导 数 相 关 ， 从 全 局 坐标 系 G 
(OXYZ) fi ee ai 应 该 能 够 看 见 该 矢量 。 导 数 变换 [ 式 
(7-152)] 是 常规 公式 ， 对 于 两 个 有 相对 运动 坐标 系 之 间 导 数 变换 的 每 个 矢量 ， 都 可 应 用 该 
公式 。 

友 例 7-17 ”旋转 矩阵 的 微分 方程 。 

对 于 所 定义 的 角速度 和 矩阵， 式 (7-5) 可 被 写成 一 阶 微分 方程 













































































人 cra 一 cRaaGn 一 0 (7-153) 
式 (7-153) BY A AY DA WA RE We 8 Es ee X, BH 
SRp=e® (7-154) 
或 者 
太一 上 一 In(CGRB) (7-155) 


KB 7-18 全 局 坐标 系 中 刚体 点 的 加 速度 。 
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位 于 全 局 坐标 系 G (OXYZ) 中 的 刚体 B (Oxyz) 角 加 速度 矢量 可 由 符号 cawa 标记 ， 
并 且 被 定义 为 cwB 的 全 局 时 间 导 数 ， 即 











Gd 


GOB eB (7-156) 
利用 这 个 定义 ， 全 局 坐标 系 中 国定 刚体 点 的 加 速度 为 
Gd 
Gap =p CoBX re) =Gan X orp +c@p xX (GOBXcrp) (7-157) 


WKB 7-19 角速度 矢量 的 另 一 种 定义 。 
全 局 坐标 系 G Ud, J, K) 中 刚体 B G, 7, O 的 角速度 矢量 也 可 定义 为 


sf[Gd) 、 (dk frGdi 、 
B =] J . L e7 | . j a 
GOB i( EP i) “i a ; if dz j) (7-158) 
iE AA : 


考虑 全 局 坐标 系 G 中 随 着 固定 点 一 起 移动 的 刚体 坐标 系 B。 将 刚体 的 固定 点 作为 两 坐标 
系 的 原点 ， 如 图 7-4 所 示 。 为 了 描述 刚体 运动 ， 可 以 描述 局 部 单位 矢量 ;、7、&。 假 设 rp 是 
刚体 点 了 的 位 置 矢 量 ， 则 有 Brz 是 具有 常量 分 量 的 矢量 。 























Brp = xi + yj tek (7-159) 
当 刚 体 移动 时 ， 仅 仅 只 有 单位 矢量 ;、7 、 相对 于 全 局 坐标 系 变化 ， 因 此 位 移 微分 的 矢 
量 可 以 表示 为 
drp =xdi +ydj +zdk (7-160) 
sk (7-160) 也 可 被 表述 为 
drp=(drp +i)it(drp *j)j+(drp + kk (7-161) 
用 式 (7-160) 取代 式 (7-161) 中 右边 的 部 分 后 ， 





结果 为 
drp=(ai + dityi + dizi* dk)it(aj + di + 
yj + dj tej + dk)j + (xk + dityk «dj +z% + dg)k 























(7-162) 
利用 如 下 单位 矢量 的 关系 : 
j e di=—i* dj (7-163) 
k+dj=—j dk (7-164) 
i e dd =— È • di (7-165) 
i e di=j + dj =k + dk =0 (7-166) ”图 7-4 全 局 坐标 系 中 随 着 固定 点 
1 .7 一) *kh=k+i=0 (7-167) 一 起 移动 的 刚体 坐标 系 
i*i=j*j=kek=1 (7-168) 
dr p 可 简化 为 


drp=(zi* dk—yj di 十 (zz di—zk + dj)j +(yk dj—zxie dk)k (7-169) 
该 方程 调整 三 矢量 积 的 形式 为 








drp =[(k + dj) 十 人， dR)I+GO + dRIX (Crityi +zk) (7-170) 
也 可 简化 为 
we: dp Vey fe. SOR We if de Ve a4 eas 
ber =| [i 。 ai | (i 本 y | | 。 bt (7-171) 
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7rP 一 GOB Xrp 


(Cdj) } „(Cdk , „(Gdi . 
B z ki 2 ` | a 
3 s$ ‘ A ‘ 7-17 
GOB (+ 3 3|( Ai i] Pef j) (7-172) 


kB 7-20 角速度 定义 式 (7-158) 的 男 一 种 证 明 。 
式 (7-158) 中 所 描述 的 角速度 定义 也 可 在 角速度 矩阵 swOa 中 将 Ra 直接 替换 ， 即 


则 有 




































































8 一 CRYGra (7-173) 
因此 ， 有 
z Gdi a Gdj ~ GdR 
r © / a 
‘ ose A i 3 d d d 
i do Sky oe fe? ak E a 
_ ` ` : ` Gdi 、Gdj , Gdk 
a DO ee A 
kel kel kK K.i K+j Kek cal ogi cae 
dt dt dt 
(7-174) 
sb (7-174) 表明 : 
Gdj 
dt 4 
Gdk 、 
Ban = x3 1-175 
GOB a 1 ( ) 
Gdi 
de 7 


WG 7-21 二 阶 导数 。 
通常 情况 下 ,Sdr/dt 在 全 局 坐标 系 G UI, J. K) 和 任何 其 他 的 刚体 坐标 系 B G, jo 
hk) 中 是 一 个 可 变 矢 量 。 因 此 ， 它 在 坐标 系 G 或 者 B 中 可 微分 。 然 而 ， 微 分 阶 数 是 很 重要 
的 。 通 常情 况 下 ， 有 
BdBdr ,SdBdr 











dt dt dt dt ris) 
作为 一 个 示例 ， 考 虑 绕 着 Z 轴 的 旋转 刚体 坐标 系 和 一 个 可 变 矢 量 cr ， 有 
Gr =;f (7-177) 
因此 ， 有 
Gd x 
Fat, =] (7-178) 
dt 
Gd coso sing 0)\(1 
| ie) <br =R} C= —sing cosp 0||0 =cosgi — sing} (7-179) 
0 0 0 
对 式 (7-179) 再 进行 求 导 ， 可 得 
POI Oo ow’ (7-180) 
dt dt p sıngı p COS@] 
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BdGdr -a 
G 2 z 
P T) os (7-181) 
现在 ， 有 
Br =R}; [tÍ |=tcosgi —tsing; (7-182) 
对 式 (7-182) 求 导 则 有 
3d 
-一 (一 1P sinp 十 cosp)i 一 (sinp 十 tp cosp)7 (7-183) 


B ® . A . A 
G (E) =§r =Rz.,L(—tg sing +cose)i — (sing +t¢ cose); | 


cosp 一 sinp 0)\/—tgsing+cose) 
一 | sinp cosp 0|| —sing— rọ cosp |=1—teJ (7-184) 
0 0 1 0 


st (7-184) 表明 : 
“drdr 3dS dr 


Tan a a ra (7-185) 


7.3 刚体 速度 


考虑 一 个 带 有 局 部 坐标 系 B (ozyz) 的 刚体 ， 该 局 部 坐标 系 B (ozyz) 在 固定 的 全 局 坐标 系 
G (OXYZ) 中 自由 移动 ， 如 图 7-5 所 示 。 刚 体能 够 在 全 局 坐标 系 中 旋转 ， 与 此 同时 刚体 坐标 系 B 
(Cozyz) 的 坐标 原点 可 以 相 于 于 全 局 坐标 系 G (OXYZ) 


的 原点 移动 。 刚 体 点 P 在 局 部 和 全 局 坐标 系 中 的 坐标 
由 式 (7-186) 关联 。 
cp 一 RbBrp 十 CCB (7-186) 


这 里 ,sdp 说 明 移动 原点 o 相对 于 固定 原点 O 的 位 置 。 
在 全 局 坐标 系 G 中 ， 点 了 的 速度 为 





Gop 一 Grp 一 CRBrp 十 Gdp 一 GO rp 十 Gd 


一 GOB(CSrp 一 Gdp) 十 Gd 一 GoBX(CGrp 一 






































Gdp) 十 Gdn (7-187) 
WEAR: 
直接 对 crp 求 导 ， 可 得 
、 Gd- = Gd. 图 7-5 在 全 局 坐标 系 G (OXYZ) 中 附 有 
G G —G = (GR B + 
ge ee ie 能 自由 移动 坐标 系 B Coryz) 的 刚体 
Sdr) ="Rp®rp +Sdy (7-188) 


从 式 (7-186) 可 得 Brp 的 表达 式 ， 然 后 取代 式 (7-188) 中 的 Brp， 可 得 


Gup 一 GRBGRTCGrp 一 Gdb) 二 Gd 一 5p(Grp 一 Gdb) 十 GdB 


=o@mpX (rp —Wdy) +4dp (7-189) 
式 (7-189) 也 可 用 相对 位 置 矢 量 写 成 : 
Cyp = can XGrpt+odz (7-190) 


276 





例 7-22 刚体 速度 的 几何 意义 。 








Y 


图 7-6 刚体 速度 的 几何 意义 
7-6 所 示 为 一 个 移动 刚体 的 刚体 点 P。 点 了 的 全 局 速度 为 
Gyp =o@mp XSrpiods (7-191) 
它 是 旋转 速度 和 移动 速度 相 加 而 得 的 一 个 矢量 ， 旋 转速 度 和 移动 速度 均 表 述 在 全 局 坐标 系 
中 。 假 设 刚体 坐标 系 在 起 始 位 置 时 与 全 局 坐标 系 重合 ， 并 且 刚 体 坐 标 系 相对 于 全 局 坐标 系 具 


有 一 个 速度 cda 。 对 于 刚体 上 的 每 个 点 ， 移 动 速度 cda 是 其 共同 的 特征 ， 但 是 对 于 刚体 中 的 
不 同 点 ， 其 相应 的 旋转 速度 cop XBrrP 是 不 同 的 。 

例 7-23 在 移动 刚体 坐标 系 中 移动 点 的 速度 。 

假设 图 7-5 中 的 点 P 在 坐标 系 B 中 移动 ， 它 由 随时 间 变 化 的 位 置 矢量 Srp (2) 说 明 。 点 
P 的 全 局 速度 是 点 P 在 坐标 系 B 中 的 速度 、 = B 相对 于 全 局 坐标 系 G 的 旋转 、 以 及 坐 
标 系 B 相对 于 全 局 坐标 系 G 的 移动 速度 的 合成 。 
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Gd. Gd. f 

py rr pds 上 RaBrpy) 
d 
=T asti uc "Rp? rp) 


=Gdn+Srp+coonX Grr (7-192) 


例 7-24 多 坐标 系 中 刚体 的 速度 
考虑 3 个 坐标 系 Bos Bis Bos WRI 7-7 所 示 。 点 P 的 速度 在 同一 坐标 系 中 应 该 可 被 测量 和 
表述 。 在 任 一 坐标 系 中 ， 如 果 点 PP 是 静止 的 ， 如 在 坐标 系 Bo 中 ， 那 么 在 坐标 系 Bo 中 矢量 ?rp 的 
时 间 导 数 是 零 。 如 果 坐 标 系 Bo 相对 于 坐标 系 B 是 移动 的 ， 那 么 矢量 irp 的 时 间 导 数 是 坐标 系 Bp 
相对 于 坐标 系 Bi 旋转 的 旋转 速度 和 坐标 系 Bo 相对 于 坐标 系 Bi 的 移动 速度 的 合成 。 在 机 器 人 正 
向 速度 运动 学 中 ， 速 度 应 该 在 基体 坐标 系 Bo 中 能 够 被 测量 。 因 此 ， 在 基体 坐标 系 中 点 P 的 速度 
是 坐标 系 B 相对 于 坐标 系 Bi 的 速度 和 坐标 系 Bi 相对 于 坐标 系 Bo 的 速度 的 合成 。 
orp 一 0d1 十 "dd 十 2rP (7-193) 
°rp =, HR d2: HRT pP (7-194) 
因此 ， 可 通过 合成 相对 速度 求 得 点 已 的 速度 。 
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图 7-7 在 基体 坐标 系 Bo 中 移动 坐标 系 Bi 中 的 移动 刚体 坐标 系 Bo 








or 一 0d1 十 (0ORiIds 十 "Ri1d2) 十 "及 22 rp= odi +? @, x4 d2+°R ido + m2 Xorp 


(7-195) 
大 部 分 时 间 里 最 好 使 用 相对 速度 的 方法 书写 ， 即 有 
wp =v: Hiv: +SP (7-196) 
因为 
一 "di (7-197) 
w =w: Xd 十 "Ri1d， (7-198) 
wp = (2 X orp (7-199) 
因此 ， 有 
Wp =? di +2e1 xd 十 "Rilds 二 os X3rp (7-200) 


例 7-25 ”速度 矢量 是 自由 矢量 。 

速度 是 自由 的 ， 所 以 为 了 在 不 同 坐 标 系 中 表述 速度 ， 我们 只 需要 通过 旋转 和 矩阵 预 乘 速 
度 。 因 此 ， 考 虑 vw; 作为 坐标 系 B; 相对 于 表述 在 坐标 系 Be 中 的 坐标 系 B 原点 的 坐标 系 B 
原点 的 速度 ， 我 们 可 以 写 出 : 








tvu; =— tv; (7-201) 
k wi = Rn” v; (7-202) 

因此 ， 有 
irp ='up =ivp +i@;jXirp (7-203) 


dz’ 
KG 7-26 FEER. 
为 了 回答 是 否 每 次 都 有 一 个 具有 零 速 度 的 点 ， 我 们 可 以 利用 式 (7-186), FFA ATW 
写成 : 








GOB(Gro 一 Gdb) 十 Gd 一 0 (7-204) 
以 便 求 得 具有 零 速 度 点 的 矢量 sr。 
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Gp =Sdy—c@ Bcds (7-205) 

然而 ， 斜 对 称 和 矩阵 sws 是 奇异 的 ， 并 且 没 有 逆 和 矩阵 。 换 句 话 说， 式 〈7-204) 是 没有 通 
解 的 。 

如 果 我 们 重新 限定 到 平面 运动 中 ， 如 XY 平 面 , 那么 op 一 ow 开 F Hcg! 一 1/o。 

此 , 在 2D 平 面 内 ， 在 位 置 sro 处 在 任何 时 间 里 都 有 一 个 具有 零 速 度 的 点 ， 可 由 式 (7-206) 


给 定 





























、 、 1 
Cro@= dgr 0) dn) (7-206) 


零 速度 点 被 称 为 极点 或 者 瞬 心 。 瞬 心 的 位 置 通常 是 时 间 的 函数 ， 其 运动 路 径 被 称 为 

KB 7-27 欧 拉 和 拉 格 朗 日 视点 ( 角 )。 

当 变 量 在 静态 全 局 坐标 系 中 可 测量 时 ， 它 被 称 为 绝对 的 视点 或 者 拉 格 朗 日 视点 。 男 外 一 
面 ， 当 变量 在 一 个 移动 刚体 坐标 系 中 可 测量 时 ， 它 被 称 为 相对 视点 或 者 欧 拉 视点 。 

在 刚体 的 2D 平面 运动 中 ， 在 下列 矢量 处 总 是 有 一 个 零 速 度 的 极点 。 









































T ee (7-207) 
w 

通过 式 (7-186) 取代 矢量 sr ， 可 求 得 刚体 坐标 系 中 极点 的 位 置 。 

GRBBro 十 Gdp 一 cd 一 @ 3 dp (7-208) 
由 式 (7-208) 可 求 得 刚体 坐标 系 中 零 速度 点 的 位 置 矢 量 Bro 。 
Bro =—SRIcõ@p Ed B=— RI (CRBERE JE 1Gdg=—CER} (CRR! )Cdg 
eR Gdy (7-209) 
因此 ， i nar aul aromas 与 此 同时 矢量 aro 表 示 刚 体 中 的 相 
ER. Reto ro FSH BA A ZR, Pr 指 欧 拉 迹 线 。 








KG 7-28 ”螺旋 轴 和 螺旋 运动 。 
针对 移动 刚体 ， 螺 旋 轴 可 以 被 定义 为 一 条 直线 ， 刚 体 上 的 点 已 具有 平行 于 角速度 矢 

Ecos Son 的 速度 。 这 样 的 点 满足 
Gup =c@rX (Erp —ĉdr)+°ds=pcø@B (7-210) 










































































Et, p 是 一 个 标量 。 因 为 Go 垂直 于 cewBX (Sr — d), x (7-209) 与 GoB 的 点 积 〈 点 
HE) 结果 是 
p=- con . Gdp) (7-211) 
引入 一 个 参数 &， 以 便 说 明 旋 转轴 上 的 不 同 点 ， 旋 转轴 的 方程 可 由 式 〈7-212) 所 定义 : 
orp=odst (c@n X dn) tkows (7-212) 
因为 如 果 我 们 有 a Xx =b Ha b =0, Ax =—a “(a Xb ) 十 ka 。 在 本 例 中 ， 有 
copX(Grp 一 Gdb) 一 pcaoa 一 Gdn (7-213) 








矢量 (cr p 一 Sd b) 垂直 于 矢量 co pX (Sr p 一 Sd bp),， 并 且 因 此 也 垂直 于 矢量 
(po ws 一 Gds)， 因 此 在 任何 时 间 的 空间 中 都 存在 一 条 直线 *， 该 直线 平行 于 sos CER 
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度 平 行 于 cws 的 点 轨迹 。 
MRs 是 直线 s 上 一 个 点 的 位 置 矢 量 ， 则 有 




















G@pX (Ss 一 db) 一 DGwB cdp (7-214) 
直线 s 以 外 任何 点 的 速度 为 
cm 一 GOBX(Ccr 一 cg )+ pca@s (7-215) 





IN (7-215) 表明 ,在 任何 时 候 刚体 点 的 速度 可 以 被 分 解 成 垂直 于 和 平行 于 角速度 矢 
量 cop 的 分 量 。 因 此 ， 刚 体 中 任何 点 的 运动 都 是 螺旋 和 运动。 参数 是 移动 速度 对 旋转 速度 
的 比率 ， 它 被 称 为 节 距 。 总 的 来 说 ，*、` GOB 和 上 闷 可 以 是 时 间 的 函数 。 


7.4 速度 变换 矩阵 
































考虑 在 全 局 坐标 系 G 中 刚体 B 的 运动 ， 如 图 7-5 所 示 。 假 设 刚 体 坐 标 系 BCozyz) 在 某 
一 初始 时 刻 to 与 全 局 坐标 系 G(OXYZ) 重合 。 在 任何 时 间 1 二 1。， 坐 标 系 B 不 必 与 全 局 坐 
标 系 GOOXYZ) 重合 ， 因 此 齐 次 变换 矩阵 5sTa G) 是 时 变 的 。 

刚体 上 的 一 点 P 的 全 局 位 置 矢量 srp() 是 时 间 的 函数 ， 但 是 其 局 部 位 置 矢量 arp 是 
个 常量 ， 等 于 crp (to). 























Brp=©rp (to) (7-216) 
B G 中 可 以 看 到 刚体 B ES P 的 速度 通过 在 参数 坐标 系 G 中 对 位 置 矢量 求 


























cup 一 和 crp(D) 一 cr (7-217) 
这 里 ,Srp 表示 全 局 坐标 系 G ae lie 的 导数 。 
全 局 坐标 系 中 刚体 的 速 通过 应 用 齐 次 变换 和 矩阵 求 得 。 
Gy (1) 一 GYBGr (t) (7-218) 


EIA OW, 是 速度 变换 矩阵 。 


E OP Ts [ane a ee ee es bis 





0 0 0 0 0 0 
(7-219) 
证 明 : 
基于 齐 次 坐标 变换 有 
Grp(t)=STg(t)Brp =CTg(t)’rp (to) (7-220) 
因此 ， 有 
Gd G . 
G —G G 
Gp = Ter, Bpp)=ST p8pp=| dz Re. Gy “ds Bp es Jor, 
dt 0 0 
0 0 
(7-221) 


替代 由 式 (7-220) 所 得 的 矢量 Srp ， 可 得 


CRp i P 一 CREGdB 





Gop 一 GTBSTELGrp(D) 一 
0 0 0 1 
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GRpGRE Ae —CRLERTGd, |. Gx Cy ea SG f 
_ B“Rg “dp B Ra ds Ge 0)=| C8 dz—c@p’dz Gpp(t) 


0 0 0 0 
(7-222) 
这 样 ， 在 参考 坐标 系 G 中 ， 通 过 预 乘 参 考 坐 标 系 G PAA R ERME ESV HA P 位 

置 矢量 可 以 获得 刚体 B 中 任何 点 P 的 速度 。 
Smop 王 SGVBcrPp(L) (7-223) 
这 里 
GV =c TB TF ie Cdy—c@,° a+) -he2 ee) (7-224) 
0 0 0 0 

c@r = Rr RE (7-225) 
Gy 一 Gdp 一 CRbGRTGdD 一 Gd 一 oO podp="dp—copX dp (7-226) 


假设 速度 变换 矩阵 Ya 是 一 个 矩阵 算 子 ， 它 能 提供 坐标 系 B (ozyz) 中 任何 点 的 全 局 
TREE, TEE ESR ESV, 由 在 全 局 坐标 系 G (OXYZ) 中 所 描述 的 角速度 矩阵 s@s 和 坐标 系 


速度 cd p 构成 。 和 矩阵 SVe 取决 于 6 个 参数 ， 角 速度 矢量 sw 8 的 3 个 分 量 和 坐标 系 速度 Gd p 
3 











的 3 个 分 量 。 有 时 容易 引入 一 个 被 称 为 速度 变换 矢量 的 6X1 矢量 ， 以 简化 数值 计算 。 
G ste aes 
se=| a lea (7-227) 
GOB GOB 


类 比 于 角速度 的 两 种 表述 ， 全 局 坐标 系 G 中 刚体 B 的 速度 可 以 由 式 〈7-224) 中 速度 变 
PRIEIES VE, 表述 ， 或 者 由 式 (7-227) 中 速度 变换 矢量 ctp 表 述 。 速 度 变换 矢量 表示 一 个 非 对 
称 矢 量 ， 因 为 cop 和 So 的 维 数 是 不 同 的 。 

HK BI 7-29 基于 坐标 变换 矩阵 的 速度 变换 矩阵 。 

基于 坐标 变换 矩阵 ， 可 以 求 得 速度 变换 矩阵 。 从 式 (7-228) 开始 。 











GR Gd 
cr(t) 一 CTBBr = Br (7-228) 
0 1 
对 式 (7-229) 进行 微分 可 得 
i Gd. i GR, Gd 
Cy 一 一 (GTBBr )=°Tp?r -| Rs “d Jr (7-229) 
dt 0 1 
Br Sse, er (7-230) 
因此 ， 有 
_ {eRs ee CR» Cd ie ‘nie, 
Ts r= r 
G GRT Gg —G GRTG 
ps RE Sd 一 GRBGRSE “| 一 GVbGr (7-231) 
0 0 


We Bil 7-30 速度 变换 矩阵 的 逆 矩 阵 。 
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由 式 (4-67) 可 得 从 刚体 坐标 系 到 全 局 坐标 系 的 变换 和 矩阵 为 

















Spr _ GpTG 
m= | (7-232) 
0 1 
依据 相同 原理 ， 我 们 可 以 引入 由 式 (7-233) 所 得 的 速度 变换 矩阵 的 道 变 换 和 矩阵 ， 即 
ayo yz! AA —(CRpBERT) = (Cd 一 GRBGRTGd | 
0 0 
-ei —GRpSRE!(Sd GRBGRIGSd | 
0 0 
| —GRpOR,'Gd e (7-233) 
0 0 
则 有 
cyVpSVs!=I (7-234) 
因此 ， 当 已 知 刚体 点 的 速度 矢量 Swvp 和 速度 变换 矩阵 SVs， 我 们 可 由 式 (7-235) 求 得 刚 
体 点 的 全 局 位 置 矢量 。 
rp 一 GYVBIGmP (7-235) 


友 例 7-31 刚体 坐标 系 中 的 速度 变换 矩阵 。 
在 全 局 坐标 系 G 所 定义 的 速度 变换 矩阵 sVa 可 由 式 (7-236) 描述 。 





GR GRT Gy —Gp.GptGa 
m-f a ee Ee | (7-236) 
0 0 


然而 ， 刚 体 坐 标 系 中 也 可 以 表述 速度 变换 矩阵 ， 即 


e al) GR» Gd = CRIGRy GREGd _|Bos Gd 
G 3 S B 3 
0 1 0 0 0 0 0 0 


(7-237) 
这 里 ,gaoas 是 刚体 坐标 系 B 相对 于 全 局 坐标 系 G 且 表 述 在 刚体 坐标 系 B 中 的 角速度 ,Sd 是 全 
局 


坐标 系 G 中 坐标 系 B 原点 表述 在 刚体 坐标 系 B 中 的 速度 。 
也 有 可 能 利用 和 矩阵 乘法 求 得 刚体 坐标 系 中 的 速度 变换 和 矩阵 。 


























Bap =ST5!Syp =T 5ST gy rp =2V 58 rp (7-238) 
利用 式 (7-218) 和 式 (7-236) 的 定义 ， 我 们 能 够 求 得 坐标 系 B 和 坐标 系 G 之 间 的 速度 
A IE RE. 
CV= TBËVgETR! (7-239) 
如 果 我 们 由 式 (7-239) 定义 变换 和 矩阵 的 时 间 导 数 ， 那 么 它 将 是 非常 有 用 的 。 
GT 一 cVBcTB (7-240) 
或 者 
GTB 一 GTpRVe (7-241) 





类 似 地 ， 我 们 可 以 由 式 〈7-241) 定义 从 连 杆 G) 到 连 杆 (i 一 1) 的 速度 变换 矩阵 。 








ig SR RT s) (7-242) 


0 0 
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二 Re Ra (7-243) 
0 0 
例 7-32 具有 固定 点 的 运动 。 
当 刚体 中 的 一 个 点 固定 于 全 局 坐标 系 中 时 ， 可 以 很 方便 地 设置 移动 坐标 系 B (Oxyx) 
的 原点 和 位 于 固定 点 上 的 全 局 坐标 系 G (OXYZ)。 在 这 些 条 件 下 ， 有 








Gds=0,°ds=0 (7-244) 
式 (7-222) 可 化 简 为 
Sop 一 GOBSrp() 一 GOBXGSrp(t) (7-245) 
il 7-33 球形 坐标 速度 。 
从 球形 坐标 系 S (Orbo) 到 笛 卡 儿 坐 标 系 G (OXYZ) 的 齐 次 变换 矩阵 可 由 式 (7-246) 





cosOcosp 一 Sinp cospSin0 rsindcose 


、 CR, Cd cos@sin cos sinosin0 rsin@sin 
Ts RegyD2.-=[ ]- id r j ý 


0 1 — sind 0 cos rcosd 
0 0 0 1 
(7-246) 
齐 次 变换 矩阵 STs 的 时 间 导 数 为 
0 一 9 6 cose r sindcos@ 
os ovsoTs (0 Tu= 0 0 O sing rsingsinp CT. 
p 一 0 cosp ”一 0 sing 0 r cosô 
0 0 0 0 
(7-247) 


KG 7-34 球形 坐标 系 中 的 速度 。 





























图 7-8 具有 关节 度量 90; M0 的 RR 平面 机 械 手 




















图 7-8 所 示 为 一 个 具有 关节 变量 91 HO. 的 RR 平面 机 械 手 。 连 杆 (1) 和 连 杆 (2) 
都 是 RR (O°) 关节 ， 因 此 变换 矩阵 "Ti T: MT: 分 别 为 
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cosO1 —siné; 0 
sing | cos 0 
T; = 
0 0 1 
0 0 0 
cos0» 一 Sin0， 0 
sind cos 2 0 
1T, = 
0 1 
0 0 
cos(@; +02) sin(O1 +02) 
sin(@, +02) cos(0ı +02) 
OT =° Tı T = 
0 0 
0 
Am 和 点 me 的 位 置 矢量 分 别 为 
ZL1cosO1 
Ll1sin01 
OF mi = 9 Ping = 
0 
1 
l2 cos(01 
OP m =P Ti Em 一 EA 


为 了 确定 点 ms HRE, RIAT, RMT AA 


=P easing 一 站 56860013 0 


Ê 12 08019 —@ sind 1 0 


T= OT, = 











lı cos, 
Lı sinO, 
0 
1 
l2 cos02 


l2 sinĝ2 

















lo cos0» 


Lə sind» 





lı cos] 











Lı sinĝı 


























(7-248) 
(7-249) 
lı cos 
Ll1sin01 
(7-250) 
(7-251) 
(7-252) 


HOT: 的 直接 微分 计算 求 得 。 


— l0 12 sind 12 一 0 1L1Sin01 


120 12 COSO 12 +6 ılı cos0, 


0 
0 


+6» 


—li sing; 
li cos), 
0 
1 
一 /2Sin0> 
L2 cos? 
0 





0 0 0 
0 0 0 
0 一 0 十 0 012=01 
也 可 利用 链 式 规则 计算 "T，: 
。 d d 。 了 
0 — 6 — “(0 1 —0 1 op 1 
T> di T? T Tı T2) TT: +°Tı!T? 
这 里 
一 Sin01 —cosé; 0 
oT, <6, cosh, 一 Sin01 0 
0 0 1 
0 0 0 
一 Sin0， 一 cos0» 0 
TEN cosĝ2 一 Sin0 0 
0 0 il 
0 0 0 
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1 


(7-253) 


(7-254) 


(7-255) 


(7-256) 


(7-257) 








EAT 和 17，， 我 们 可 以 通过 利用 "Ti A Ty) 求 得 速度 变换 矩阵 "V 和 1V。 。 














cOSO1 sing} 0 一 /1 
= 一 Sin01 cosd, 0 0 
oT, = (7-258) 
0 0 1 0 
0 0 0 1 
coOSO， sinf 0 一 /， 
_ 1 —sin@z cos0» 0 0 
T; = (7-259) 
0 0 i 0 
0 0 0 1 
0 一 1 0 0 
| 
0V = "Fy Ti =o (7-260) 
0 0 0 0 
0 0 0 0 
0 一 1 0 0 
,| .Il 0 00 
1V =!T,'T,'=0» (7-261) 
0 0 0 0 
0 0 0 0 
AG. kimi 和 点 me 在 坐标 系 Bo 和 Bo 的 速度 分 别 为 
一 /1Sinb01 
. lı cos, 
Umi = V1 rm =O 1 A (7-262) 
0 
一 /2Sin0， 
l2 cos02 
lyn2 =V! Fm2 =0 2 0 (7-263) 
0 











为 了 确定 末梢 点 m 在 基体 坐标 系 中 的 速度 ， 我 们 可 以 使 用 速度 矢量 加 法 。 


一 (b 1 二 02)12 sinCO1 +0:)—ĝ ılı sinô 


9 m2 = Umi FI Um = Um HUn = (9 +4 2)lzcos(0ı +02) + ı Lı cosb: (7-264) 
0 
0 
我 们 也 可 通过 速度 变换 矩阵 "V， 确定 "ww? 。 
ym2 = V2 OP m2 (7-265) 


这 里 Ve 为 
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0 = er 0 0 11sin01 

oy, =° T0 T7! = ĝi +ô: 0 0 —0 lı cnet (7-266) 
0 0 0 0 
0 0 0 0 


cos(0ı F02) sin(Q1+02) 0 —lz—li cose 
—sin(0ı +02) cos; +42) 0 l1sin02 
1 
0 





oT; ='97 To =!T; OT]! = 


0 0 0 
0 0 1 
(7-267) 
我 们 也 可 使 用 加 法 规则 "V: =° V HIV 确定 速度 变换 矩阵 "V，。 
0 —01—0, 0 0 ol1sind, 
0V, 一 "0V1 +V; = 0 十 0， 0 0 —6 ,11cos01 (7-268) 
0 0 0 0 
0 0 0 0 
这 里 
0 一 05 0 0 211 Ss 
oV, = T1 V Ti! = ĝo 0 0 一 0 21100861 (7-269) 
0 0 0 0 
0 0 0 0 
因此 ,ws 为 
一 (01 十 6;)7asin(0 十 9) 一 0171singi 
E (6 1 +6 2)lzcos(01 +02) +8 ı Lı cosh (7-270) 
0 
0 


7.5 REMEE Se Be 





从 齐 次 连 杆 变换 矩阵 中 可 直接 求 得 速度 变换 和 矩阵。 根据 正 向 运动 学 ， 存 在 一 个 4 义 4 齐 
次 变换 矩阵 以 实现 每 两 个 坐标 系 之 间 的 变换 。 




















ri riz? 713 ra 

cRB Gd 721 r22 r23 ra 
Tg = = (7-271) 

0 1 r31 732 733 T34 





当 变 换 和 矩阵 中 的 元 素 是 时 变 的 ， 其 导数 为 
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dri dr 12 dr 13 dria 
dt dt dt dt 
Gg dro, dr2» dre3 dr 
i 一 GT 一 GT 一 | dt dt dt dż (7-272) 
dral dr32 dr33 dr34 
dt dt dt dt 
0 0 0 1 
调整 变换 矩阵 的 时 间 导 数 ， 使 之 与 变换 矩阵 成 比例 ， 即 
GT 3=SVp°T (7-273) 
这 里 ,SVs 是 一 个 的 4X4 齐 次 和 矩阵， 被 称 为 速度 变换 矩阵 或 者 速度 算 子 矩阵 ， 有 
ei de GRESRE Gd 一 GRBGRTGd 
GVB 一 GTBGTH = (7-274) 


0 
基于 速度 变换 矩阵 的 齐 次 变换 矩阵 及 其 导 
关节 相连 接 的 两 根 连 杆 而 言 ， 其 变换 矩阵 的 导 



































1 
数 在 正 向 速度 运动 学 中 特别 有 用 。 对 于 由 旋转 
BUT; 为 








一 Sin0; 一 cosgicosa; cosĝ;sina; —a;isinĝ; 
。 COSO ; — sinĝ;cose; sind; sina; a;cosd; 
1 下 ;一 0 (7-275) 
0 0 0 0 
0 0 0 0 
对 于 由 棱柱 关节 ( 平 动 关节 ) 连接 的 两 根 连 杆 而 言 ， 其 变换 矩阵 的 导数 一 1T,; 为 
0 0 0 0 
”lo 00 0 
1 人 ;一 ; (7-276) 
0 0 di 
0 
旋转 关节 的 速度 变换 矩阵 为 
0 一 1 0 
. : : - |1 0 0 0 
i1V ,=6 Ar=0; (7-277) 
0 0 0 0 
0 0 0 0 
棱柱 关节 CRO) 的 速度 变换 矩阵 为 
0 0 0 0 
. : “I0 0 0 0 
i—1V, =d ;Ap=d; (7-278) 
0 0 0 1 
0 0 0 0 
iE AA : 
因为 任何 变换 矩阵 都 可 以 被 分 解 为 平 动 和 转动 
Ras d I d)\(Ra, 0 
r=("" )-( re )=pr (7-279) 
0 1 0 1 0 1 
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我 们 可 以 求 得 工 为 





[Rew 4 |- Id les | I=[1+D ][1+R ]—1=Vvr (7-280) 
o 1 bo ilo 1 
其 中 V 是 速度 变换 和 矩阵， 描述 为 




















一 下 下 一 1 -EE d ii —Ri,sd J-F aa d aaar d a 






























































0 0 0 1 0 1 0 1 
(7-281) 
机 器 人 中 两 相 邻 坐标 系 之 间 的 变换 矩阵 用 基于 D-H 参数 的 方程 [ 式 (5-11)] 描述 。 
cos0; —sin@; cosa; sind; sina; a; cosd; 
; sind; cosl; cosa; —cos6;sina; a;sind; 
a = (7-282) 
0 sina ; cosa ; d; 
0 0 0 1 
直接 求 导 表明 ， 如 果 通 过 旋转 关节 将 两 根 连 杆 连 接 起 来 ， 那 么 9; 是 D-H 和 矩阵 的 唯一 参数 ， 因 此 ， 有 
—sinĝ; —cos0;cosa; cosd;sina; —a;sinĝ; 
cos0; —sin@;cosa; sind; sina; a; cos6; 
ame Ue =ù; 
0 0 0 0 
0 0 0 0 
0 —1 0 0 
eV 0 0 oj : : 
=ù; ~1T,=6@ Api IT; (7-283) 
0 0 0 0 
0 0 0 0 
IN (7-283) 表明 ， 旋 转速 度 变换 矩阵 为 
0 一 1 0 0 
1 0 0 0 
i“1V,=6 ;Ar=0; (7-284) 
0 0 0 0 
0 0 0 0 
然而 ， 如 果 通 过 棱柱 关节 连接 两 根 连 杆 ，4d; 是 D-H 矩阵 中 唯一 的 变量 ， 因 此 有 
0 0 0 0 
0 0 0 Oj. $ . 
iIT;=d; i—lIT;=d;Api IT; (7-285) 
0 0 0 1 
0 0 0 0 
IN (7-285) 表明 ,棱柱 关节 的 速度 变换 矩阵 为 
0 0 0 0 
. 0 0 0 0 
i-lV,=d j;Ap=d; (7-286) 
0 0 0 1 
0 0 0 0 
Ar Fil Ap 是 在 机 屁 人 速度 分 析 中 具有 一 些 应 用 的 旋转 速度 系数 矩阵 和 平 动 速度 系数 矩阵 。 
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例 7-35 变换 矩阵 的 微分 。 
假设 给 定 了 变换 矩阵 为 


T= 


oF O O 
Oo je 
= A A A 


0 
1 
0 
0 


sk (7-287) 服从 于 由 式 (7-288) 和 式 (7-289) 所 确定 的 旋转 微分 和 平 动 微分 


dgu=(0.1 0.2 0.3) 


dd=(0.6 0.4 0.2) 
TAI, FEHR ABE ek ot dT 为 





0 —0.3 0.2 0.6 
0. 3 0 — 0x 4 
dT =(1+dD) (I+ dR) — I= 
—0.2 Ql 0 0. 2 
0 0 0 0 


例 7-36 平 动 微分 和 转动 微分 。 


假设 绕 旋 转轴 w 的 旋转 角 也 是 很 小 的 ， 由 dg 表示 ， 那 么 旋转 矩阵 的 微分 为 








1 —u3d¢ uz dọ 
d 1 —uid 
I+ dRe.g =I+Ra.a3 = 7 á ji i 
—u2d¢ uid¢ 1 
0 0 0 
因为 当 8 二 <1 时 ， 有 
sing ~d¢ 
cosg ~ 1 
vers¢ ~0 
平 动 微分 dd =d (dal +d, 十 d:K) 可 由 平 动 矩阵 微分 表示 。 
1 0 0 dds 
0 dd, 
I+dD= ` 
0 0 1 dd: 
0 0 0 0 
因此 ， 有 
0 —dgus  dduz 
dg$us 0 —ddui 
dT =(1+dD) (I+ dR) — I= 
—dduz ddu 0 
0 0 0 


例 7-37 主 微分 旋转 的 合成 。 
SEX. Y, Z 轴 的 旋转 微分 分 别 为 


0 


0 
0 
1 


(7-287) 


(7-288) 
(7-289) 


(7-290) 


(7-291) 


(7-292) 
(7-293) 
(7-294) 


(7-295) 


(7-296) 
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1 -0 0 0 
ees o aa (7-297) 
0 dy 1 0 
0 0 0 1 
1 0 dB 0 
0 1 0 0 
Re (7-298) 
0 0 0 1 
1 一 da 0 0 
Rz, a = M : e (7-299) 
0 0 1 0 
0 0 0 1 
因此 ， 绕 着 X、Y、2 轴 旋 转 的 主 微分 旋转 矩阵 的 合成 矩阵 为 
[LITRx.ay JLITRy,a LITRz.a ] 
1 0 0 0 1 0 dB 0\f1 一 da 0 0 
ofo 1 -dy ol o 1 0 Offde 1 0 0 
lo dy 1 oll—æ o 1 olo o 1.0 
0 0 0 1 0 0 0 1/J\0 0 0 1 
1 —da d 0 
= 7 H 4 i =(1+R 2, ca [I+Ry,a [I+Rx.a7] 
0 0 0 1 
(7-300) 
所 以 ， 微 分 旋转 矩阵 的 合成 具有 互 换 性 。 
例 7-38 罗 德 里 格 斯 旋转 公式 的 导数 。 
基于 罗 德 里 格 斯 旋转 公式 ， 角 - 轴 旋 转 和 矩阵 为 
Rog =Icosg +uu! versé +using (7-301) 
因此 ， 罗 德里 格 斯 旋转 公式 的 时 间 导 数 为 
Ra. =—dcos¢1+aa' $ sing +u¢ cosg =$URa.s (7-302) 


kB 7-39 基体 坐标 系 Bo 中 坐标 系 B 的 速度 。 
相对 于 基体 坐标 系 Bu ， 在 连 杆 Gi) 上 建立 坐标 系 B, MIRR B; 的 速度 可 通过 求 导 基 
体 坐 标 系 中 坐标 系 B: 的 位 移 "d; 而 求 得 。 
od 
~ dt 


od 
on; °d; =—(CT;'d;) 
dt 
=0 Ti Toi 7 ds HOT Ts? Tye? T; id; + 


«+T 1T2?T3 lT; id; 


i goT;. \. 
= (> iy |ia: (7-303) 


j= 1 9g; 
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然而 ， 偏 导 可 由 速度 系数 矩阵 A; 求 得 ，A; 是 4R 或 者 Ap. 








=A;' IT; (7-304) 


因此 ， 有 





(7-305) 


ƏT; OTit T2? Tai ?2T; Aji Tjee IT; (Gj <i) 
997 | 


Te Gn 
例 7-40 Wmd =0, 那么 V REH o, THEHR. 
考虑 具有 共同 原点 的 两 个 坐标 系 B 和 G。 在 这 种 情况 下 , 4d =0, xt (7-278) 将 变 为 





E | k n ki H 
T= = = =IR=R (7-306) 
o 1) lo 1 0 1 
因此 ， 变 换 和 矩阵 T 的 导数 为 

T=R (7-307) 
所 以 ， 速 度 变换 矩阵 V 等 效 于 @。 








V=TT~'=RR'=o (7-308) 
Gil 7-41 两 个 具有 共同 原点 坐标 系 之 间 的 D-H FE 
如 果 两 相 邻 坐标 系 具 有 相同 的 坐标 原点 ， 那 么 式 (5-11) D-H 变换 矩阵 中 的 a; Mdi 是 














零 。D-H 矩阵 可 简化 为 
cos0; —sin0;cosa; sind; sina; 0 
7, sind; Ne —cos@;sina; 0 eae 
0 Slna ; cosa; 0 
0 0 0 1 
我 们 可 以 去 掉 这 个 和 矩阵 的 最 后 一 列 和 最 后 一 行 ， 通 过 旋转 变换 矩阵 说 明 。 
cos0; 一 Sin0i cosa; sind; sina; 
‘~1R;=| sind; cosĝ; cosa; —cos0; sina; (7-310) 
0 singa; cosa; 











Sai Mdi 为 零 时 ， 两 根 连 杆 则 由 旋转 关节 连接 ， 因 此 0; 是 D-H 和 矩阵 中 的 唯一 变量 ， 
所 以 有 














一 Sin0; —cos@;cosa; cosOiSinai 
i1R; =0;| cos0; —sinO;cosa; singisina; |=i-1@;'—!R; 
0 0 0 
Or =i O 
=ġ,;|1 0 of 1R;=6;'-1Kj-1'R; (7-311) 
0 0 0 
式 (3-310) 表明 ， 旋 转角 速度 矩阵 为 

0 1 0 

-10;=6;|1 0 0o|=0; ki (7-312) 
0 0 0 
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7.6 ASR eH 








如 果 刚 体 坐 标 系 B 与 坐标 系 G 具有 共同 的 坐标 原点 ， 并 且 相 同 坐 标 系 G 连续 旋转 ， 那 











么 变换 矩阵 与 时 间 有 关 。 
Sr (1)=°RE()Pr (7-313) 
这 时 ， 坐 标 系 一 个 点 的 B 中 全 局 速度 为 
Crt) =S9 G) ="Rp(t)®r 一 GOBGr (7-314) 
这 里 ,cwoB 是 反对 称 角速度 矩阵。 
0 一 03 we 
cõsr = Rz R] =| ws 0 —wi (7-315) 
“w? wi 0 


矩阵 cOB 与 角速度 矢量 co 一 8x 相关 ， 它 等 于 绕 着 瞬时 旋转 轴 u 的 一 个 角 速 率 册 。 在 
基体 坐标 系 Bo 中 ， 通 过 相 加 机 器 人 连 杆 的 角速度 可 求 得 杆 (nm) 的 角速度 。 


n 























Ww, =00 ı +10: t90; H Hi0 一 2) o; (7-316) 
i=] 
为 了 知道 相对 移动 连 杆 的 角速度 ， 我 们 需要 在 刚体 坐标 系 和 全 局 坐标 系 中 依照 相对 导数 
规则 求 导 : 
B . an an Pet 
Opp rp Pop =iityj +À (7-317) 
Gq . Sh. ts e 
g PP re =Corp=XI+YI+ZK (7-318) 
cd 7 . 
DX rr (7-319) 
Bd ye 
Gp ore =orp +o@nX orp =Erp (7-320) 
在 移动 坐标 系 了 中 点 P 的 全 局 位 置 矢量 为 
Grp =°Rg’rp +°dpg (7-321) 


点 卫 的 全 局 速度 为 
Cup ="rp =ca@pn (orp —“dp) +odp =cøgX (rp —8dy)+odp (7-322) 
刚体 B 在 全 局 坐标 系 G PR ITE Ze Wl HE ae oy. A Se Jy ROBEY GD ak Fe PE h E 
阵 5Vs 表述 。 





cm (1) 一 GYBGr (t) (7-323) 
这 里 ,SVs 包括 坐标 系 B 在 全 局 坐标 系 G 中 的 平 动 速度 和 转动 速度 。 
GOB A 


0 0 


GVB 一 GTBGT1 -| (7-324) 


习题 
7-1 标记 与 符号 。 
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描述 下 列 这 些 符号 的 含义 : 
1) GOB 2) BOG 3) Sog 4) Ras 5) ROG 6) FOG 


7) ol 8) 5@: 9) 3@; 10)°Rp 11) $@1 12) tø; 


‘ cd Bd 
13) Srp (t) 14) 5 Sup 15) AR 16) Ap 17) T 18) Ay 

Sd Cd, Pdi Ge Gg G 
19) TE rp 20) ae rp 21) T rp 22) rp 23)Sdp 24) “Vp 


7-2 局 部 位 置 、 全 局 速度 。 
一 个 刚体 以 一 个 定常 锥 角度 绕 着 一 个 全 局 主轴 旋转 。 试 求 在 Br 处 一 点 的 全 局 速度 。 
Br 一 (5 30 10)? 

1) 全 局 主轴 是 Z 轴 ， 当 a 二 30"， 角 速率 是 a 王 2rad/s。 

2) 全 局 主轴 是 了 轴 ， 当 p= 二 30"， 角 速率 是 二 2rad/s。 

3) 全 局 主轴 是 X BH, 4 y¥=30°, PARR Y =2rad/s. 

7-3 参数 化 角速度 、 全 局 主轴 旋转 。 

刚体 坐标 系 B 在 全 局 坐标 系 G 中 的 旋转 。 旋 转变 换 矩 阵 可 以 被 分 解 为 主轴 旋转 。 确 定 
角速度 cwpb 和 camwp。 

1) SRs 是 刚体 坐标 系 B 先 绕 着 Z 轴 旋 转 a 角 接着 绕 着 Y 轴 旋 转 B 角 的 结果 。 

2) SRs 是 刚体 坐标 系 B 先 绕 着 Y 轴 旋 转 有 8 角 接 着 绕 着 Z 轴 旋 转 a 角 的 结果 。 

3) SRB 是 刚体 坐标 系 B 先 绕 着 Z 轴 旋 转 a 角 接着 绕 着 X 轴 旋 转 y 角 的 结果 。 

4) SRs 是 刚体 坐标 系 B 先 绕 着 X 轴 旋 转 y 角 接着 绕 着 Z 轴 旋 转 a 角 的 结果 。 

5)SRs 是 刚体 坐标 系 BERE X 轴 旋 转 y 角 接 着 绕 着 Y 轴 旋 转 8 角 的 结果 。 

6)SRsE 是 刚体 坐标 系 B 先 绕 着 Y 轴 旋 转 8 角 接着 绕 着 X 轴 旋 转 y 角 的 结 

7-4 数字 化 角速度 、 全 局 主轴 旋转 。 

一 个 刚体 B 在 全 局 坐标 系 G 中 旋转 。 其 旋转 变换 矩阵 可 以 被 分 解 为 主轴 的 旋转 。 针 对 
习题 7-3, AA a =2rad/s, È =2rad/s, ý =2rad/s Æ a =30°, B=30° Fl y= 30°”, 确定 角 速 
Eor Moos. 

7-5 全 局 位 置 、 定 常 角速度 。 

一 个 刚体 以 一 个 定常 角 速 率 绕 着 一 个 全 局 主轴 旋转 。 试 求 ar 处 一 点 在 上 一 3s 之 后 的 全 局 
速度 ， 如 果 刚 体 坐 标 系 和 全 局 坐标 系 在 上 一 0s 时 刻 重合 在 一 起 。 

Br 一 (5 30 10)7 

1) 全 局 旋转 轴 是 Z 轴 ， 角 速率 是 a =2rad/s. 

2) 全 局 旋转 轴 是 了 轴 ， 角 速率 是 二 2rad/s。 

3) 全 局 旋转 轴 是 X 轴 ， 角 速率 是 y 一 2rad/s。 

7-6 RE x 轴 旋 转 。 

当 刚 体 坐 标 系 绕 着 刚体 轴 旋 转 时 ， 试 求 角速度 矩阵 。 

1) 局 部 旋转 轴 是 x 轴 ， 角 速率 是 @ =2rad/s, HEE p=45°. 

2) 局 部 旋转 轴 是 x 轴 ， 角 速率 是 0 =2rad/s， 角 度 是 0 一 45"。 














































































































速 
速 : 
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3) 局 部 旋转 轴 是 x 轴 ， 角 速率 是 yj 二 2rad/s， 角 度 是 y= 二 45”。 

7-7 ”合成 旋转 与 合成 角速度 。 

对 于 以 定常 速率 绕 着 全 局 旋转 轴 旋 转 的 刚体 坐标 系 了 来 说 ， 试 求 其 旋转 矩阵 并 计算 刚 
体 坐 标 系 B 在 全 局 坐标 系 G 中 的 角速度 。 

D 先后 绕 着 的 全 局 旋转 轴 分 别 是 Z 轴 、X 轴 、Y 轴 ， 相 应 的 旋转 角 分 别 为 : 绕 着 2 轴 
旋转 30"， 绕 着 X 轴 旋 转 30°, SEB Y 轴 旋 转 90"。 相 应 的 角 速 率 分 别 为 : 绕 着 Z 轴 的 角 速 


Ka 一 20"/s， 绕 着 X 轴 的 角 速 率 8 =—40°/s, BEM Y 轴 的 角 速 率 7》= 二 55°/s。 

D 先后 绕 着 的 全 局 旋转 轴 分 别 是 X 轴 、Y 轴 、2Z 轴 ， 相 应 的 旋转 角 分 别 为 : 绕 着 X 轴 
旋转 30"， 绕 着 立轴 旋转 30"， 绕 着 Z 轴 旋 转 90"。 相 应 的 角 速 率 分 别 为 : 绕 着 X 轴 的 角 速 
Ra 一 20"/s， 绕 着 立轴 的 角 速 率 6 =—40°/s, BH Z 轴 的 角 速 率 》=55"/s，。 

3) 先后 绕 着 的 全 局 旋转 轴 分 别 是 了 轴 、Z 轴 、X 轴 ， 相 应 的 旋转 角 分 别 为 : 绕 着 X 轴 
旋转 30"， 绕 着 立轴 旋转 30"， 绕 着 Z 轴 旋 转 90"。 相 应 的 角 速 率 分 别 为 绕 着 X 轴 的 角 速 


率 =20"/s， 绕 着 站 轴 的 角 速 率 8 = 一 40"/s， 绕 着 Z 轴 的 角 速 率 》 一 55*/s。 

友 7-8 全 局 三 重 角速度 矩阵 。 

对 于 附录 A 中 的 全 局 三 重 旋转 ， 确 定 角速度 swB Meor. 

友 7-9 局 部 三 重 角 速度 矩阵 。 

对 于 附录 B 中 的 局 部 三 重 旋 转 ， 确 定 角 速度 cws Moen. 

7-10 ”表达 在 局 部 坐标 系 中 的 角速度 。 

在 局 部 坐标 系 B (Ozyz) P, -MS P 位 于 rp 二 (1，2，1) 处 。 

1) 当局 部 坐标 系 以 角 速 率 7 = 75°/s 绕 着 全 局 X 轴 旋 转 30"， 接 着 以 角 速 率 a =25°/s 绕 
着 全 局 Z 轴 旋 转 45" 时 ， 试 求 EOa 。 

2) 当局 部 坐标 系 以 角 速 率 a =25°/s 绕 着 全 局 Z 轴 旋 转 45"， 接 着 以 角 速 率 》 =75°/s # 
着 全 局 X 轴 旋 转 30" 时 ， 试 求 E@8。 

7-11 全 局 翻滚 角速度 、 俯 仰角 速 遍 航 角速度 。 

对 于 下 列 的 一 个 全 局 翻滚 、 俯 仰 、 航 旋转 来 说 ， 计算 角速度 c@s。 


1) a =20°/s, B=—20°/s, ý =20°/s, a=30°, B=30° F y=30°, 




































































a 














2) a =0°/s, B=—20°/s, ¥=20°/s, a=30°, B=30°H ¥=30", 
3) a =20°/s, B=0°/s, ¥ =20°/s, a=30°, B=30°M ¥=30°, 
4) a =20°/s, B=—20°/s, ¥=0°/s, a=30°, B=30° Fl Y=30°, 

















5) a =0°/s, B=0°/s, ¥=20°/s, a=30°, B=30°A y=30°, 
7-12 NTRS FG RISE MARE, mifa RE 
全 局 翻滚 角速度 、 俯 仰角 速度 和 偏 航 角 速 率 : a =20°/s, B =—20°/s, y=20°/s, FHA 





下 列 旋转 和 矩阵， PORES, FG Op 


1) 
0.53 一 0.84 0.13 
e| 0. 0 0.15 :| 
一 0.85 —0.52 0.081 
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系 o 


2) 
0.53 一 0.84 0.13 
0.0 0.15 0:99 
—0.85 —0.52 0.081 


CRp= 








7-13. BRIERE F Pi. 
7-9 HIR NA Fie oh RAAE Si Bs 是 在 手腕 点 处 的 一 个 全 局 固定 坐标 
对 于 下 列 情况 ,确定 末 端 执行 器 坐标 系 Br 的 角速度 soy 。 











图 7-9 运动 中 的 欧 拉 手腕 








D 只 有 第 1 个 电动 机 绕 着 zs 轴 以 04 旋转 。 

2) 只 有 第 2 个 电动 机 绕 着 z MA Os 旋转 。 

3) 只 有 第 3 个 电动 机 绕 着 zs 轴 以 66 旋转 。 

第 1 个 电动 机 绕 着 = 轴 以 04 旋转 ， 同 时 第 2 个 电动 机 绕 着 zs 轴 以 0 旋转 。 

第 1 个 电动 机 绕 着 zs 轴 以 904 旋转 ， 同 时 第 3 个 电动 机 绕 着 zs 轴 以 06 旋转。 

第 2 个 电动 机 绕 着 z 轴 以 0s 旋转 ， 同 时 第 3 个 电动 机 绕 着 zs 轴 以 O 5 旋转 。 

第 1 个 电动 机 绕 着 z 轴 以 04 旋转 ， 第 2 个 电动 机 绕 着 zs 轴 以 0; 旋转 ， 同 时 第 3 














4 


VY 


5 


VY 


6 


VY 





7 


VY 


个 电动 机 绕 着 zs 轴 以 6。 旋转。 











7-14 由 罗 德 里 格 斯 公式 所 得 的 角速度 。 
由 SRs 一 Ra,y 我 们 可 以 得 到 其 时 间 导 数 为 
d 
dż 
利用 罗 德 里 格 斯 旋转 公式 ， 试 求 coOB AR op 。 

7-15 ”和 斜 对 称 和 矩阵 。 

证 明 : 任何 方 阵 都 可 以 被 表述 为 对 称 和 矩阵 和 和 斜 对 称 和 矩阵 之 和 。 


1 1 
A=B+C B= (A+A?) C 一 了 (4 一 人 ) 











cRB 一 





GREp= 
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友 7-16 旋转 滑 块 。 
图 7-10 所 示 为 在 旋转 臂 上 的 一 个 滑 块 。 









图 7-10” 转 杆 上 的 滑 块 


Cd Gd7 Gadk 
dt’ dt’ dt 
Gd2î Gd27 Gd2k 
dt? ° dt?’ dt? ° 
按照 下 列 的 混合 导数 规则 ， 试 求 滑 块 质量 m 在 其 质心 处 的 局 部 速度 Bw 和 局 部 加 速 


度 Ba , LAR RG a n = 











EDs 


Glide’) @id jy) ola” 


友 7-17 全 局 微分 和 局 部 微分 。 


Gd 
dt 








考虑 在 Srp 处 的 一 个 局 部 点 。 局 部 坐标 系 BAS Z 轴 以 a 角速度 在 全 局 坐标 系 G 中 旋 
Bd Gq. Bd. 
转 。 计算 二 rP， Grp, g reM Te 


1) Brp=ti +j 
2) Brp=ti+tj 
3) Brp =t? i +t] 
4) Brp=ti+t?j 
5) Brp =ti +t] Hth 
6) Brp =ti +t? j +tk 


7) Brp=isint 








8) Br p =i sint +j cost +k 


K7-18 极 坐标 机 械 手 的 速度 分 析 。 

















图 7-11 所 示 为 具有 关节 变量 0 Md 的 平面 
极 坐 标 机 械 手 。 

确定 "Ti IT °T2 OV 1V2 0V 和 机 械 手 
的 末梢 点 的 速度 。 

K7-19 角速度 的 斜 对 称 和 矩阵 恒等式 。 

UE AR : 


m 








ROR? =R © 图 7-11 平面 极 坐 标 机 械 3 
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友 7-20 角速度 指数 的 变换 。 
证 明 : 





Gok = RgO Re 


W7-21 角速度 矩阵 恒等式 。 





D+H =(—1)tw tg 
K7-22 球形 机 械 手 的 速度 分 析 。 
7-12 所 示 为 具有 关节 变量 91 02 Md 的 球形 机 械 手 。 

















图 7-12 具有 关节 变量 01 、0。 Ad 的 球形 机 械 手 





MEV 1V2, Va, OVa, OV; 和 机 械 手 末梢 点 的 速度 。 
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机 器 人 的 速度 分 析 可 以 划分 为 正 向 速度 运动 学 和 逆向 速度 运动 学 。 已 知 关节 变量 的 时 间 
变化 率 ， 求 末端 执行 器 在 全 局 坐标 系 中 的 笠 卡 儿 速 度 ， 这 就 是 正 向 速度 运动 学 。 已 知 末 端 执 
行 需 的 速度 ， 确 定 关节 变量 的 时 间 变 化 率 ， 这 就 是 逆向 速度 运动 学 。 


8.1 刚性 连 杆 速度 


工业 机 器 人 中 的 每 根 连 杆 相 对 于 其 相 邻 的 连 杆 ， 都 有 一 个 角速度 @ 或 者 平 动 速度 4d， 如 
图 8-1 所 示 。 在 全 局 坐标 系 Bo P, ÆI G) 的 角速度 是 连 杆 G) 的 全 局 角速度 之 和 ， 对 
Fi. A 





1o; 一 1O; (8-1) 


Ohja OFF RÆ) 
| 0 (对 于 了 关节 ) 






关节 (2) 
AFG) 


i 
lle: 
! 
1 
1 
了 


关节 ;71 人 
\ 
A 8-1 








= xj Kiti 
































有 相对 速度 的 3 个 被 连接 的 移动 连 杆 


C) 


nea E 


第 8 章 速 





EI (i) 上 所 建 坐标 系 B;， 其 坐标 原点 在 基体 坐标 系 中 的 速度 为 
(对 于 R 关节 ) 
(8-3) 
(对 于 了 关节) 





0 0 
00; X ,id; 


.00 .一 
7 ak 十 9 X 0 
iRi-1 To Wi X ;-\ di 

















里 , OMd 是 D-H 参数 ，d 是 坐标 原点 的 位 置 矢 量 
因此 ， 如 果 一 个 机 器 人 有 n 根 连 杆 ， 那么 最 后 坐标 系 的 全 局 角速度 是 
00， = 2 :1 0; (8-4) 
连 杆 坐标 系 的 全 局 速度 矢量 为 
ody = få, (8-5) 
(8-6) 


iE AA: 
依据 D-H 的 定义 ， 坐 标 系 B: 相对 于 坐标 系 Bi 的 位 置 矢 量 为 
td; =d hy) +a; 
因此 ; 9d; MEG: 的 函数 。 
这 时 ， 通 过 旋转 关节 连接 到 连 杆 




























































































对 于 ;三 i， 式 (8-6) 取决 于 关节 变量 g;， 因 
假设 除了 关节 i 之 外 ， 机 右 人 中 的 每 个 关节 都 被 定 锁 。 
i 一 1) WEI G) 角速度 为 
lo =b h WR i 是 RR 关 节 , 且 是 唯一 可 移动 的 关节 (8-7) 
然而 ， 如 果 连 杆 G) 和 连 杆 (i 一 1) 通过 棱柱 关节 而 相连 接 ， 则 有 
;_?@; 二 0 如 果 关 节 是 i 是 P 关节 , 且 是 唯一 可 移动 的 关节 (8-8) 
相对 位 置 矢量 [ 式 (8-6) ] 表明 ,通过 旋转 关节 连接 到 连 杆 (i 一 1) 的 连 杆 Gi) 角 速 
关节 i 是 PP 关节 ,日 是 唯一 可 移动 的 关节 
(8-9) 














度 为 
2d: =0%R ,_, Xa = 1@; X; d; 如 果 关 


RMH RR a, A x; 轴 绕 着 z;_1 轴 以 角速度 6; 旋转 ， 因 此 G x 





寸 棱柱 关 节 而 相连 接 ， 则 有 



























































d°k;-,=0, Siti, WR G) 和 连 杆 (i 一 1) 通过 
;4d; 二 dk; 1 如果 关节 i 是 P 关 节 , 且 是 唯一 可 移动 的 关节 (8-10) 
现在 ,假设 所 有 和 较 低 的 关节 Gi) 都 是 移动 的 ， 那 么 连 杆 G) 在 基体 坐标 系 中 的 角 
速度 为 
Jo 一 >) 1 ao， (8-11) 
j=l 
或 者 
Oki 1 (对 于 RR 关 节 ) 
= > i 1 TF R 关节 (8-12) 
0 (对 于 了 关节 ) 
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式 (8-12) 也 能 被 写成 递归 形式 ， 即 
00; =) @,_, HiO; 
或 者 
°@, ,+0%:-1 (对 于 RR 关节 ) 


0 = 
00; 一 


00; (对 于 了 关节 ) 
JERE G) 坐标 系 原 点 在 基体 坐标 系 中 的 速度 为 
00; X;—i d; (对 于 R 关节 ) 
dhit @; X,_9d; OTF PY) 
n FEAL SEA HP E Ja E AF OF oh E E AA EE Ra 文 些 结果 的 直接 应 用 。 
友 例 8-1 串联 刚性 连 杆 的 角速度 。 


0 了 — 
oe: ~ 
































考虑 一 个 具有 n IMn 个 旋转 关节 的 串联 机 械 手 。 依 据 前 述 连 杆 的 角速度 ， 连 





的 全 局 角速度 为 
"0; =o; 1 + O°R 1 
或 者 更 一 般 地 ， 有 


i 
= Aor 
i= >) OR; 
j=l 





因为 
o= b ki 
20i =b; ki 
20i = ?0 TOS; O; 1 十 0， rei 2=0; Thi uS 
因此 ， 有 


=] 


ee De ‘@;- 1+ 0 = Dk 1 +Å? É; = D igi- 1 


* fil 8-2 申 联 刚性 连 杆 的 平 动 速度 ， 
依据 前 述 连 杆 的 角速度 ， 串 联机 械 手 中 连 杆 〈z) 的 全 局 角速度 为 





og, =! vii +; 1@: X i di 
这 里 
0v;=° d; 
= >) Ckj-1X, Ydi)0, 
j=l 
因为 


0 0 
i—1 Vi — 00; X i— d; 


0 = 
i—2Vi-—1 T00; 1X; 5d; 1 
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(8-13) 


(8-14) 


(8-15) 


(8-17) 


(8-18) 
(8-19) 
(8-20) 


(8-21) 


(8-22) 


(8-23) 


(8-24) 


(8-25) 
(8-26) 











0. — o0 o — 0 Oy — 0 0 
5-2 Vi =; Vi + Vi = 2 B-1 Fo; X; id =0@i-1 X; di +0; X ;-1 d; 


=0, hig xX, 9d;_1 +6, "É; x; Tdi 






































(8-27) 
因此 ， 有 
1 一 1 $ 
y; = 5 j-19 +; vi; 一 > (oF; 1X, 94.)0; (8-28) 
j=l j=l 
K% Gl 8-3 基体 坐标 系 中 的 递归 速度 。 
FREMA T 的 时 间 导 数 可 以 被 调整 为 
T=VT (8-29) 
这 里 , 工 可 以 是 连 杆 变换 矩阵 或 者 机 器 人 正 向 运动 的 结果 ， 例 如 : 
OTs = Til1T, ?Ts33 Tt Ts Te (8-30) 
因此 ， 当 计算 速度 变换 矩阵 TT 时 ， 可 计算 变换 矩阵 的 时 间 导 数 。 
变换 矩阵 "T; 为 
OT; =9T1!T22T3? IT; (8-31) 
和 矩阵 "Y 和 "Wi;+1 为 
oy, =0 T T7! (8-32) 
Vier ATAS OTTA Th = CTT HTT TH (8-33) 
这 两 个 方程 可 以 被 合成 为 一 个 递归 公式 : 
oVi+1 一 0Vi HTV; OT; (8-34) 
依据 连接 两 连 杆 的 关节 类 型 ， 可 以 简化 递归 速度 变换 矩阵 公式 。 
对 于 旋转 关节 ， 速 度 变换 矩阵 为 
0 一 1 0 0 
. . 1 0 0 0 . i—ik;ı 0 
IVi =qitiArR=6 41 AR=89 i41 =6 54) (8-35) 
0 0 0 0 0 0 
0 0 0 0 
这 时 ， 有 
00;41 =o; +0541 =00; 十 0 Rit (8-36) 
式 (8-36) RH., MERR B;+1 的 角速度 是 坐标 系 B: 角速度 加 上 由 关节 变量 g;+1 而 产生 的 相 
对 角速度 。 而 且 ， 有 
soe eR creme Bie ley d;.,—°d;) (8-37) 
sh (8-37) 表明 ， 平 动 速度 可 通过 坐标 系 B; 平 动 速度 加 上 由 连 杆 坐标 系 Bi;+1 旋 转 所 产生 的 


速度 而 获得 。 
对 于 棱柱 关节 ， 速 度 系数 矩阵 公式 为 
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0 0 0 0 
; : . |0 0 0 0 0 Sie 
Vit+1=gi+1AP=di+1AP=di+1 =diti= (8-38) 
0 0 0 1 0 0 
0 0 0 0 
因此 ， 有 
Qi+1 =O; (8-39) 
odit: =? di Tomi C di+1 —° di) +d; this (8-40) 





sk (8-39) MIR (8-40) 表明 ， 坐 标 系 Bii 的 角速度 与 坐标 系 B; 的 角速度 是 相同 的 。 而 且 ， 
通过 将 由 到 +1 而 产生 的 平 动 速度 加 上 由 连 杆 坐标 系 Bi 的 旋转 而 产生 的 相对 速度 可 以 获得 
平 动 速度 。 





机 器 人 正 向 速度 运动 学 主要 解决 已 知 关节 速度 4g 求 末端 执行 器 速度 辟 的 问题 。” 自由 度 
机 器 人 的 关节 速度 矢量 g 是 一 个 nX1 矢量 。 


g=(g Gn Gn gn)T (8-41) 














并 且 ， 一 般 情 况 下 ， 末 端 执行 器 速度 矢量 证 是 一 个 6X1 RE, 


; i ; od, Un 
X=(X, Yn Zn wx, vwy, | j=| | (8-42) 








未 端 执行 器 速度 矢量 义 中 的 元 素 线性 正比 于 关节 速度 矢量 g 中 的 元 素 ， 即 
X=Jg (8-43) 
其 中 ，6Xn 比例 矩阵 J(q) 被 称 为 机 器 人 雅 可 比 和 矩阵。 
坐标 系 B， 原 点 速度 "ww, 的 全 局 表达 式 正 比 于 机 械 手 关节 速度 gp H 





°w, =Jn qo Eq (8-44) 
3Xn 比例 矩阵 Jp(q) 是 机 械 手 位 移 雅 可 比 和 矩阵 。 














9d,(gp) aT(g) 

















D 5 (8-45) 
Iqp oq 
坐标 系 By 角速度 ow, 的 全 局 表达 式 正比 于 g 的 旋转 分 量 。 
0@n=JSR q (8-46) 
3Xn AESC) 是 机 器 人 旋转 雅 可 比 矩 阵 。 
9 
J a (8-47) 
aq 





我 们 可 以 综合 式 (8-44) MIÈ (8-46) 以 表明 机 器 人 正 向 速度 运动 学 。 
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证 明 : 
对 于 一 组 给 定 的 关节 速度 4; (i 二 1，2，…，n)， 正 向 速度 运动 学 主要 用 来 确定 末端 执 
行 器 的 平 动 速度 "vw, 和 转动 速度 "w,。 平 动 速度 矢量 "wv, 和 转动 速度 矢量 "w, 的 分 量 正比 于 关 


节 速 度 q: G=l, 2, =, n), 





°v, =Jpg (8-48) 


0@,=JIRq (8-49) 
H KER J o 和 JR 被 称 为 位 移 雅 可 比 矩 阵 和 旋转 雅 可 比 和 矩阵 。 
综合 式 (8-48) 和 式 (8-49) 有 








X=J gq (8-50) 
在 此 ， 要 分 别 定义 雅 可 比 矩 阵 、 被 称 为 末端 执行 器 速度 矢量 和 和 关节 速度 矢量 g 。 
-| ”| (8-51) 
JR 
. om， 
=, (8-52) 
On 
gq=(g, In In aad gn)T (8-53) 


无 论 什么 情况 ， 当 我 们 分 析 无 手腕 机 器 人 的 速度 运动 学 时 ， 都 可 以 通过 J 表示 Jp。 
考虑 6 自由 度 机 器 人 ， 其 中 3 自由 度 机 械 手 用 来 定位 手腕 点 ，3 自由 度 球 形 手腕 用 来 定 
向 末端 执行 器 。 在 末端 执行 器 坐标 系 Bs 和 基体 坐标 系 Bo 中， 点 的 坐标 变换 为 















































I Od) Rs 0) (Re ds 
="Ts (q )° roir =| | ]-[ J (8-54) 

0 1 0 1 0 1 
XE, BKET 是 6 个 关节 变量 qi G=1, 2, 3, , 6) 的 函数 。 我 们 总 可 以 将 6 个 关 
节 变 量 分 解 为 末端 执行 器 定位 变量 gl go. gs 和 末端 执行 器 定 问 变量 gs gs. qoo AMT 
行 器 定位 变量 是 位 置 矢 量 "ds 的 唯一 变量 ， 末 端 执行 定 器 定向 变量 是 旋转 变换 符 阵 "Rs 中 的 

唯一 变量 。 
ds =° de (q1 > q2» q3) (8-55) 
Re =° Re (q4, qs, ge) (8-56) 
末端 执行 器 坐标 系 Bo 的 原点 位 于 5r 王 0， 该 矢量 在 全 局 坐标 系 中 的 矢量 位 置 为 


Rs ld; \(0 I "di 10 0 
0 p= = =° D, = dg (8-57) 
0 1 1 0 1 1 1 


末端 执行 器 位 移 矢量 "ds =(X Y 2] 的 分 量 是 机 械 手 关节 变量 gi、g2 Alas. 
X di(gi, q2» q3) 
Y |=|d2(q1; q2» q3) (8-58) 
Z d3 (q1; q2» q3) 


对 式 (8-58) 两 边 求 导 ， 导 数 方程 说 明 速 度 矢量 %ws =r WEEE g q2, qs 的 线性 
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E ,9d1. Adı. 
Gg age a 

u addy. drs Ado. 

Y=, 9175, 027 5,, 93 (8-59) 
aqi og2 aq3 








u Gds Jde daeu 
Z= q3 





3qı 2! 3agy42 9g3 
式 (8-59) WHH. us EXTERE qi, qo. qs 的 线性 组 合 。 
3de s 3de 7 3de 











Ga = a aes | 
Vi 一 JpD 9gD 王 dl1 Tq2 TA q3 a (8-60) 
RATE FY A AR E ME BEE Pe A A I BEE RR 
ddc. š 
"w= gg d= ID qD (8-61) 








=| Nae (8-62) 














位 移 雅 可 比 和 矩阵 Jp 等 效 于 变换 矩阵 了 对 机 械 手 关节 坐标 的 导数 。 
dds PDs PT; IT) 















































Jp P P P By (8-63) 
末端 执行 器 的 角速度 为 
0@5 一"RORT (8-64) 
然而 ， 旋 转变 换 和 矩阵 的 时 间 导 数 为 
。 d 
Re =p ORU Re Rs R R5’ Re) 
Ri 1p.2p 3P 4P 5 “0 9 Re 2P 3P 4P 5 ”0P 1 Rs 3R 4P 5 
一 gl a R2°R3°RytRs°Re + q2°Ri ape “R3°R, R5°Re+q3 RI R: - Rt R;°” R6 
x IR, g P ARs . 5 ; I Rg 
qı 2 INS 5 Re Tq; Rl 2 3 ANG 6Tqs Kı 2 R3 Rd 5 
十 q4R1R2 Ra ——'R5°R "RI Ro? Rs? Re S— Retae Ri Ro? Rs? Ri Rs — 
od 4 od5 aqe 
(8-65) 
wees EMER, 的 转 置 为 
ORE = OR RRISRAR Ro) = RERISRIR3IRIRT (8-66) 
FAK ows =° RIR]. BI 
OR 0 : IR, 7 a2R 0 
006 一 41 可 ~ RT 二 dsoRi R? +q; Rs， 3 二 R? 
qı q2 q3 (8-67) 
7 3R 0 í a R;° 3R 0 
+q R; 可 Ri+gs Rs 7 Ri +q R; 7 : RE 
q od5 oqs 
006 一 0 O11 a oo, + > 0: + Py o, + i @; + ny Os (8-68) 
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ODE To EI WIRE gi G=1, 2, 3, ++, 6) 的 一 个 线性 组 合 。 















































: Vows Vows Vows Pos + Pus, Jiwe 5 
0o06 SJR gq 5q1 EPa Fq2 Ig a Oe on Hqs TA 上 as6 (8-69 
这 里 
3? 四 OR 0 
“一 一 一 pT (8-70) 
9g1 9g1 
2 ms IIR? 
=R, R; (8-71) 
9g2 9gq2 
20 ms IR? 
一 "R， R! (8-72) 
9g3 9g3 
go a JR 0 
ESOR; 一 一 (8-73) 
dqa dqa 
3? > r 94Rs° 
一 0R， R! (8-74) 
9g5 Ids 
2° mwl IER? 
ORs R] (8-75) 
Iq6 Iq6 
综合 平 动 速 度 和 转动 速度 ， 可 以 利用 式 (8-50) RAR HL AR A RESO 
OD, Jo). è 
dg 一 J 9 (8-76) 
"a, JR 
BLAS AE AY EE KB ME 
ds Id, Vd 
5 ê : ê 3 > 0 0 
qı aq2 q3 
= (8-77) 
90@6 90o06 Pos; 9om06 9om06 06 
9g1 gdq 9g3 9g1 9gs 996 





如 果 机 器 人 及 根 连 杆 和 TR 


般 的 情况 ， 6Xn 














ERY HEERE J 变 成 ， 




















Vd, Vd, Pd, 2° d, 
9 aq: 9 adn 
— qı q2 q3 q (8-78) 
Jo ©, Jo ©, Jo ©, Jo Qo, 
9g1 9g2 9g3 Ign 
例 8-4 平面 极 坐 标 型 机 械 手 雅 可 比 和 矩阵 。 
8-2 所 示 为 一 个 具有 下 列 正 向 运动 学 的 平面 极 坐标 型 机 械 手 。 
cos 一 Sin0 0 0 0 0 r cosSO 一 Sin0 0 rcosO 
sind cos? 0 00 1 0 0 sind cos? 0 rsing 
"To =° Tı T2 = = 
0 1 O1/0 0 1 0 0 0 il 0 
0 0 0 1/0 0 0 1 0 0 0 1 
(8-79) 
机 械 手 的 末梢 点 的 位 置 矢量 为 
X recos 
=| | (8-80) 
Y rsind 
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和 应 用 机 强人 学 : ey. WHY SAR 











TH 











图 8-2 平面 极 坐标 型 机 械 寻 





x i om] r 
. |=! . A (8-81) 
Y sinf rcosd 0 
式 (8-81) 表明 : 
aX 9X 
dr 90 cos0 —rsin0 
Jo= co (8-82) 
9Y day sinf rcos0 


ar 90 
有 唯一 旋转 关 方 坐标 9。 旋转 矩阵 "Rs 说 明 ; 


002 一 0R20RIT 一 0 (8-83) 























因此 ， 有 
w1 0 
002 =| w: |=|9 (8-84) 
a 
ee (8-85) 
JR 一] (8-86) 
cosO —rsind : 
Y 一 | sinf recos ;| (8-87) 
i" 0 t i 
= 、 图 8-3 2R 平 面 机 械 手 
例 8-5 2R 平 面 机 械 手 的 雅 可 比 
HERE, 


有 具有 两 个 || R 连 杆 的 2R 平面 机 械 手 如 图 5-9 和 图 8-3 所 示 。 该 机 械 手 的 正 向 运动 在 
fi] 5-8 中 已 经 进行 了 分 析 ， 在 例 6-3 中 进行 了 道 向 运动 分 析 。 
ÉF (1) MEI (2) 的 角速度 为 
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正 向 运 


001 一 010R0 
0 一 0 o$ 
102 2 Ro 


00: 一 0 o ti p=; + 02)2ko 
机 械 手 末梢 位 置 的 全 局 速度 为 


od: =° di =) d: =0 @ X? di +0 @ X? dz 
一 012&0 Xili £00, +42) ho X11s 


=1101071 X12(01 40997 
通过 应 用 坐标 变换 法 可 求 得 单位 矢量 "7 和 "7，。 


cosO1 一 Sin01 0 一 Sin01 

°F1=Rz, 0171= | sin0) cos coSsO1 
0\(0 
0 111 
1) 八 0 





0 
cos(0ı +02) sin(0ı +02) 
07 2 =Rz, 9, 10 27 3 一 Sin(O1 +82) cos(@; +02) 












































(8-91) 


(8-92) 


—sin(@; +02) 
=| cos(O1 +62) (8-93) 


0 0 0 
取代 式 (8-91) 中 的 单位 矢量 ， 则 有 
一 Sin0l —sin(@, +02) 
"ds 一 0101| cos: |X12(01+02)| cos(O1 +62) (8-94) 
0 0 
重新 调整 ， 则 有 
X = —l,sin0; —lysin(O; +62) 一 2Ssin(CO +42) 
| 。 l2cos(@, +42) If | -( | a 
利用 2R 机 械 手 简 化 的 结构 ， 我 们 可 以 找到 其 更 简单 的 雅 可 比 和 矩阵 。 所 求 得 的 机 械 手 的 
云 动 学 矩阵 为 
cos(0ı +02) sin(@; +62) 0 Licos01 十 02cos(CO1 +02) 
sin(@, +02) cos(0ı +02) 0 Lısinĝı +losin(@; +02) 
T: =° Tı T2: = (8-96) 
0 0 1 0 
0 0 1 
式 (8-96) 表明 ， 机 械 手 的 末梢 位 置 矢 量 0d? 为 
- Wo 
一 (8-97) 
Y 2, sinO; +l2sin(@, +02) 
对 式 (8-97) 直接 求 导 ， 则 有 
9 a +02) 
= (8-98) 





Y L101cosO1 +2201 PO +02) 
将 式 (8-98) 调整 成 矩阵 形式 ， 有 
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X) /lsingi—/l2sin(01+0s) 一 02sin(Oi 十 9) 人 2 
y LicosO1 十 lz2cos(01 +02)  Lzcos(ðı +02) 八 0， 
或 者 
X=J160 (8-100) 
Jp 是 2R LRF AIE nT HEFE RE, 
OX ax 
901 90， 一 /1sin01 一 /2Sin(O +02) 一 /2Ssin(O1 十 0，) 
Jp= == (8-101) 
9Y IY ZicosOl +l2cos(O, +02) Z2cos(CO1 +02) 
901 302 
利用 连 杆 的 绝对 倾斜 角度 91 、01 H02, RATE E WO 2R 机 械 手 的 速度 方程 写成 : 
X 一 isingl —l2sin(@;+62)\("1 
p= 本 (8-102) 
Y Lı cos] lacos(0ı +02) 6, +65 





fl 8-6 2R BL bE HE AT be A AY ZI 








2R 平面 机 械 手 的 雅 可 比 和 矩阵 可 通过 应 用 列 - 列 方法 系统 地 求 得 。 上 坐标 系 的 全 局 位 置 矢 量 
































出 


分 别 为 
{do =12i2 (8-103) 
0d: =l? i +l2?i? (8-104) 
因此 ， 有 
d2 =o @ı x? d: +? @2 xI d: =0°ko X C1021 十 12022) 十 020ki X12°1, 
i f (61 (8-105) 
=(°ho X U1? i1 +L 2%%2) OR, X12°i2 } 
式 (8-105) 可 被 设置 成 下 面 的 形式 。 
0 də OR x? d: og1 XX dz A, . 
| 上 A . | 三 J0 (8-106) 
0 @2 ko ki 02 


例 8-7 关节 式 机 械 手 。 





8-4 所 示 为 一 个 关节 式 机 械 手 。 机 械 手 的 连 杆 是 R 上 FR(90")、R 有 RC0") MR ER 





(90 )， 以 及 它们 在 坐标 系 之 间 的 相伴 变换 矩阵 为 


cosO1 0 sind; 
sing; 0 一 cosOl 
oT 一 
0 1 0 
0 0 0 
cos0» —sing,» 0 
sind» cos 2 0 
IT 和 
0 0 1 
0 0 0 
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0 
0 
(8-107) 
li 
1 
l2 cosl2 
Z2sin02 
(8-108) 
0 
1 











图 8-4 RFRIR 关 节 式 机 械 手 




















cos; 0 sing; 0 
27, = sind; 0 一 cos0O3 0 1605 
0 i 0 0 
0 0 0 i! 
用 点 P 表示 机 械 手 的 末梢 点 。 点 P 的 全 局 坐标 位 置 矢 量 为 
Xp cosOi[ cos0 +13 sin(O2 +43) | 
opp Yp oT, sinfı Ll2 cos02 +13 sin(02 TAN atis) 
Zp ls lı — L3 cos(02 十 03 )+l2sin2 
Į 1 1 





矢量 "rp 的 坐标 必须 用 于 确定 机 械 手 的 雅 可 比 和 矩阵 。 

OXP 9Xp IX p 
901 90， 903 
OY p OY p JY p 























J= T T T (8-111) 
IZ p IZp IZ p 
901 202 903 
ax 
59 = Lea sin@z +03) +12.cos02 ]singi 
1 
IY 
zg = Lessin(2 +03) +2 cos0 ]cosh 
901 
9Zp 
-0 (8-112) 
ax 
<P =[l3cos(82 +03) —L2 sinz JcosO1 
IO» 
3Y 
gg Ts cos(0z +03) —Losind2 ]sinðı 
302 
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IZp . 
gg, 13C T03) Hla cosh: 
(8-113) 
AP y i 
J0; 3 cos(82 十 03 ) cosO1 
Yr, aes 
20; 3cos(02 十 03 ) sind, 
3Zz 
J0; a3 sin(02 十 03 ) 
(8-114) 


— (l3 sinf23 +l2cos@2)sin@;  (l3cos@23—l2sin82)cosé; 13cos823cos01 
J =| (l3sin023 +12 cos02)cosd, (13c0s023 —lysin@s)sin@,;  13cos023sin@; | (8-115) 
0 L3 sinf23 +l2cos@2 L3 sinf23 


023 =02 +03 (8-116) 


8.3 HET pE EIk R i 


JEN EIE EE SDR ERE, AT EB a FAB LE 
X=Jg (8-117) 

















雅 可 比 和 矩阵 为 








oko Xo d, ok1 Xx! d, at Rai Xd, 
J= (8-118) 


op OZ OZ 
ko Ry sen lka 








FN XP HA a OR EP SE AT EEE J. J 中 第 i 列 被 称 为 雅 可 比 生成 矢量 ， 用 符号 c: (gq) 


表示 。 
0Ri-1Xi Yd, 
ci(q)= 、 (8-119) 
ki 


为 了 计算 雅 可 比 矩 阵 的 第 i 列 ， 我 们 需 
HRR E; d, 和 号 ; 1。 这 些 矢量 是 共同 
表达 在 同一 坐标 系 中 的 连 杆 (i 一 1) 坐标 系 
原点 位 置 和 关节 轴 单 位 矢量 。 

基于 雅 可 比 生成 矢量 ， 计 算 WH, TE 
向 速度 运动 学 矩阵 是 机 器 人 正 向 运动 的 


结 





























证 明 : 

假设 "qd ;和 "qd ;-1 分 别 是 坐标 系 Bi 和 
Bi;-1 的 全 局 位 置 矢 量 ， 然 而 一 :di; 是 坐标 系 
B; 在 坐标 系 了 -1 中 的 位 置 矢 量 ， 如 图 8-5 
所 示 。 这 3 个 位 置 矢量 依据 矢量 加 法 而 相 图 8-5 连 杆 (i) 及 其 关联 坐标 系 
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第 8 章 ”速度 运动 学 





K., MHR (8-6), A 























"d: =° di- 十 "Rd 一 0， +d i°ki-1+a;%%; (8-120) 
对 式 (8-120) 进行 时 间 求 导 ， 得 
07 0 . 10 一 1 0 lg. 
d; d;-| R: -1i ld; H?R;— 'd; (8-121) 
=° d;i- H° R; (dii! k; —1 +a; ti;i) HOR; 1d;' k; 
式 (8-121) 说 明 坐 标 系 B: 原点 的 全 局 速度 是 连 杆 坐标 系 B;-1 平 动 速度 和 角速度 的 函 
数 。 然 而 ， 由 于 有 
id; = td; —° d= (8-122) 
OR; 1d; =p @;—1 X°R;-1' ld; =o oX; 9d: =0; b; 1X; 9d: (8-123) 
OR ad; R Sd; R; i ha Sd R (8-124) 
因此 ， 有 
ee RX; ditd; Ri (8-125) 
因为 在 每 个 点 处 0 或 者 d 是 可 变 的 ， 我 们 得 出 如 下 结论 : 
;1d; 一 0 @; X; idi OF RRB) (8-126) 
或 者 
ay —adiok it? OX cid; OFF PRT) (8-127) 
ROMY, ASG AAT AB EE BY RE Oy 
d= D 9d = DD Obi x dy (8-128) 
i=l i=l 
= oS (8-129) 
i=l i=l 
重新 调整 式 (8-128) 和 式 (8-129) 并 将 其 写成 矩阵 形式 ， 有 
ô, 
0d， 0 hee Kid, a oko XG d, 0, x? d, pines of,_1X, Yd, A» 
n i=] | Ri oko ok one RI : 
a, 
(8-130) 
我 们 通常 用 一 个 简写 符号 表示 这 个 方程 。 
X=Jq (8-131) 
这 里 ， 矢 量 g= (aq. q2 qna) 是 关节 速度 矢量 ,J 是 雅 可 比 矩 阵 。 
Ro X Sdn of1 X Idn D ok, 1 oem 
= : ~ 、 (8-132) 
oho i 
实际 中 ， 我 们 用 列 对 列 的 方法 求 得 雅 可 比 和 矩阵 。 每 列 都 是 一 个 雅 可 比 生 成 矢量 ，c; (qs 


























它 与 关节 :相关 。 如 果 关节 :是 旋转 关节 ， 则 有 
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of;_1X, d, 
ci Cg) 一 、 (8-133) 
OR 1 
WRG i 是 棱柱 关节 CEARD, WMA 
“(a)-| | (8-134) 


式 (8-117) 提供 了 一 个 含有 6 个 方程 
的 方程 组 。 最 前 面 的 3 个 方程 与 末端 执行 器 
关节 速度 的 平 动 速度 相关 。 其 余 方程 将 末端 
执行 器 坐标 系 的 角速度 与 关节 速度 相关 联 。 

例 8-8 ERIE AL AF AY AE AT Le AE PF 

图 8-6 所 示 为 一 个 球形 机 械 手 。 为 了 求 









































得 其 雅 可 比 矩 阵 ， 我 们 从 确定 ";-1 轴 开始 ， 
这 里 ， i=l, 2 3, 0 果 我 们 使 用 齐 次 定 
义 ， 问 题 将 会 变 得 比较 容易 。 
0 
0 
ko = (8-135) 
1 
0 
coOSO1 0 一 Sin01 0)\/(0 一 Sin01 
本 sing | 0 coOSO1 0 |10 cos01 
°R, =T 1k = => (8-136) 
0 =], 0 Zo |{ 1 0 
0 0 0 1 人 NO 0 
cosĝı cos? 一 Sin01 cosO1ising» 0 110 cosOl Sin0， 
sinĝı cos cos: sinĝısinĝ 0 ||O sinfı sinz 
0ko = "Teka = . 一 (8-137) 
— sind 0 cos 2 Zo |} 1 cosé2 
0 0 0 1 0 0 
这 时 ， 必 须 估计 矢量 ，?d， 
d 3cos01sin0» 
j ‘ : d3sin0, sin0， 
odz =lo° ko +d3°k2 = (8-138) 
lo +d3cos02 
0 
d3cos01 sin0， 
d 3 sin01 sin0» 
1d; =d3°k,= (8-139) 
d 3cos0» 
0 





因此 ， 机 械 手 的 雅 可 比 矩 阵 为 
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—dsinĝı sinf? caicosOlicosO， cosOli sin0， 








cascosOlsin0， Cssin0lcosOy Sin0l sin@2 
E bo Xo ds "1X? ds Re T 0 —d3sin0z cos0» 
-| fo of, ,|- 0 — sin01 0 
0 cos 0 
1 0 0 


(8-140) 





例 8-9 ST ALAA AE AT E FE 
6 自由 度 关 节 机 器 人 的 雅 可 比 矩 阵 是 6X6 ERE. LAE A MU 6-6 所 示 ， 例 6-5 中 计算 了 
其 变换 矩阵 。 
AE AT EGFR REA i 列 ，ci (Cg ) 为 
(Phi Xi ?ds 
aa =| Opies | (8-141) 


对 于 雅 可 比 和 矩阵 的 第 1 列 ， 我 们 需要 求 得 "&。 和 "ds «zo 轴 在 基体 坐标 系 中 的 方向 单位 






































0 
ok 一 |0 (8-142) 
1 





末端 执行 器 坐标 系 Bo 的 位 置 矢 量 是 "ds ， 它 可 由 变换 矩阵 "Te 的 第 4 列 直接 求 得 。 


ti tig t13 t14 





TOT TATE TTS TE 4 ja e i a (8-143) 
0 0 0 1 
这 里 
t14 
"ds =| t24 (8-144) 
t34 


tı4 =d; {sinfı sins sin?s +cosôıLcosh, sins cos(O2 +63 ) +cosds5sin(@2 +63) ]} + 
l3cosĝı sin(0 +03) 一 da Sin01 +L2 cos cosO， 

t24 =d6{—cos6 sinĝ4 sins 十 sin01[Lcosbousin0s cos(0 十 03 ) 十 coso5 sin(0。 +63) J} + 
/3Sin0lsin(02 十 03 ) 一 dcosOl 十 02Sin0l cosO， 

134 一 di6(CcosOusin05sin(02 +03) —cos05 cos(O2 十 03 )) 十 /2sin0 十 /scos(02 +03) 








(8-145) 
因此 ， 有 
0 t14 — tea 
0ko X? dg =|0|X| tea |=] tu (8-146) 
1 t34 0 


第 1 个 雅 可 比 生成 矢量 为 
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“ta 
14 
(* a] 0 
a= i = (8-147) 
obo 0 
0 
1 
对 于 第 2 列 ， 我 们 需要 求 得 "Al 和 ?ds 。 基 体 坐 标 系 中 的 z1 轴 单位 矢量 为 
0 cos01 0 Sin0l 0 sin, 
o$ =9R,1h,="9R,|0]=] sind; 0 —cosd; 110|=| —cosd, (8-148) 
1 0 1 0 1 0 


e WHT — "ELC Rk X de 。 矢 量 Y ds 是 末端 执行 器 在 坐标 系 Bi 中 的 位 置 ， 然 而 它 必须 能 在 
基体 坐标 系 中 表述 ， 以 便 执行 义 积 。 一 个 较 容易 的 方法 可 用 来 求解 "kf1 X de， 并 且 将 其 结果 
转换 至 基体 坐标 系 。 矢 量 1d6 是 来 自 例 6-5 变换 矩阵 "Te 一 "TIT22Ts3T44T55Te 的 第 4 














ij, BH 
Lo cos» +13 sin(02 +43) 
ds =| l2sind2 — Lz cos(02 +03) (8-149) 
d2 


因此 ，ez 的 前 一 半 为 


0 12cos02 十 /3sin(02 +03) 
Of, xy ds =°R, Cb, x! d;)=° Ri 0X Zo sinz — Ll3 cos(02 十 03 ) 





1 d2 (8-150) 
cos0, | 一 /sin0 +l; cos(0: +43) | 


=| sind, | —Z2sinO2 +l; cos(02 +63) | 














locos +13 sin(O2 +63) 
c2 为 
cos; (一 2Sin0 十 13cos(C0O2 +43 ) ) 
sind; (—l2sin@2 十 /3cos(C02 十 03 ) ) 
eA *) 12 cosO2 +13 sin(02 十 03 ) 
C2 >= r = (8-151) 
ok] sin, 
—cos6 
0 








Hk: Mod RGB 3 列 。 矢 量 3d6 是 坐标 系 B 中 来 端 执行 器 的 位 置 ， 变 换 矩 阵 
PTs =° Ts’ Ta Ts Ts 的 第 4 列 。 可 求 得 基体 坐标 系 中 x2 轴 的 单位 矢量 为 














0 sin) 
0 有 一 0R 2 一 0RIIR， 0 | 一 —cos6 (8-152) 
1 0 
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将 ?有 X ?qs 的 结果 变换 至 基体 坐标 系 ， 可 求 得 ,Xdo 的 叉 积 》 
ZL3cosO3 
2k, X2d5 =| 13sin03 
0 
l3 cosûı sin(@2 十 03 ) 
bs KX dg =9Ro Che X266) 一 | LgsinOi sin(O2 +63) 


















































一 /3cos(0O2 +63) 
因此 ，cs 为 
/3cosOl sin(@2 十 03 ) 
Ls sinfı sin(@2 十 03 ) 
RsX2 ds) | 一 scos(02 十 03) 
a-f Of, 上 sinĝı 
一 cosO1 
0 
第 4 AR: 和 de 。 将 ?As 变换 至 基体 坐标 系 ， 可 以 求 得 矢量 "%&s 为 
0 cos0; (cos@2 sinf; 十 Sin0zcosO3 ) 
0 有 3 一 0R11IR22R3|0| 王 | sind: (cosgssin0s 十 singzcos03 ) 
1 —cos(62 +63) 
ci 的 前 一 半 可 通过 计算 3&s X3 do 并 将 其 结果 转换 至 基体 坐标 系 中 求 得 。 
0 0 0 
°R; Ck X? ds)="Rs||0]X] 0 | )=/0 
1 L3 0 
因此 ， ci 为 
0 
0 
ok XY ds 0 
“-| o$, - cos0; (cosoy sin03 + sind» cosl; ) 
sind; (cos@2 sinf; + sin@2 cosl; ) 
—cos(@2 十 03 ) 











EIEEE, Mids. Hiki 变换 至 基体 坐标 系 ， 可 以 求 得 矢量 "%&, 为 




















0 sinfı cos, 一 cosOl sind, cos(0, +43 ) 
0$, =°R,| 0 | =| 一 cosgicos04 — sin) sindycos(G2 十 03 ) 
1 —sinĝ4 sin (02 十 03 ) 
cs 的 前 一 半 可 通过 计算 4&4, X do ， 并 将 其 结果 表述 基体 坐标 系 之 中 求 得 。 
0 0 0 
"Ri Xda] 一 "Ri 0] X]0] |=] 0 
1 0 0 


因此 ， C5 为 


(8-153) 


(8-154) 


(8-155) 


(8-156) 


(8-157) 


(8-158) 


(8-159) 


(8-160) 
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c= 


oks X? de 
of, 


上 


ZBL ts 


tt oS 





计算 "Ps APRs X ode 可 求 6 列 。 


HES Rs 为 


0 
0 
0 
sind; cosO4 — cos@; sin? cos(G2 +93 ) 
—cos@; cos, — sinfı sind, cos(0, +43 ) 


—sinO,sin(@2 十 03 ) 























(8-161) 





0 — cos; cosy sin(0 +63 )— sin, (sin@; sind, + cosd; cosh, cos(O2 +63 ) ) 
0; =R; |0 |=] —sinOicosO4sin(O2 +63) — sinb, (— cosh: sin04 +sinlı cosh4 cos(02 +3 )) 
1 cos64cos(@2 +63) 一 0. 5sin(@2 +8; ) sin204 
(8-162) 
cs 的 前 一 半 可 通过 计算 5E5 X5de ， 并 将 其 结果 表述 基体 坐标 系 之 中 求 得 。 
0 0 0 
ORs Oks X5 dg )=Rs||0]X]0] |=|0 (8-163) 
1 0 0 
因此 ，c6 为 
0 
0 
0 


一 cosOl cosOusin(02 +03 )—sin@4[ sin; sind, 十 cosOl cosh, cos(0 +63) | 





一 Sin0l cos, sin(@2 44 





Fe Lae A AY AE BT EEE E W 
J=(Ce 








03) sinĝ4 L 


C2 C3 C4 


例 8-10 ”球形 手腕 对 雅 可 比 和 矩阵 的 影响 。 
对 于 具有 球形 手腕 机 器 人 来 说 ， 其 雅 可 比 和 矩阵 为 





A ð š 
oko Xd; OR 


bo 


| 





x 
of, 





0 
ids 
oÈ, 


cosO4 cos(02 +63) 一 0. 5sin(@2 +8; ) sin204 


C5 


of, X ode 


cos01 sin04 +sin@1cos04cos(62 +43) | 





(8-164) 

cs) (8-165) 
0 0 0 

A 、 (8-166) 
Of, Of, Of. 


sk (8-166) 表明 其 右上 3X3 TEREFE, REA N ERIE F RAMU REM FBS 


作为 手腕 坐标 系 Bay Bs 和 Bs 的 原点 。 
友 例 8-11 





使 用 直接 求 导 方法 的 关节 机 械 手 ， 其 雅 可 比 矩 阵 。 





6-6 所 示 为 一 个 具有 例 6-5 所 给 出 变换 矩阵 的 关节 式 机 械 手 。 使 用 正 向 运动 学 变换 的 


结果 为 
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tl 

= t2 

1 ]- t3 
0 


ti3 Lt14 
oe (8-167) 
t33 t34 
0 1 
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我 们 知道 末端 执行 器 的 位 置 矢量 为 











Xe ti 
ds =| Y; |=|t24 (8-168) 
Zs t34 
这 里 


t14 =d; {sinfı sinOs sin?s 十 cosOlLcosOusin0scos(0 十 03 ) 十 cos05sin(0 十 03 ) ] ) 十 
/3cosOl sin(@2 +63) +d2sin@; 十 12cosOl cosO2 
t24 =d6{—cos61 sinf, sins 十 sin01[Lcosbousings cos(0 +63 ) 十 coso5 sin(0。 +63) J} 十 
/ssin0l sin(02 十 03 ) 一 docos0O1 十 /Sin0lcos0y 
134 一 di6(CcosOusin05sin(02 +03) —cos05 cos(O2 十 03 )) 十 /sin0 十 13cos(02 十 03 ) 
(8-169) 








对 Xs 进行 时 间 导 数 ， 则 有 


aXe: aXe: aXe: : ; : 
Xs = iq a, reese] 59, Tutt tekts 








=n Pe eosti l- uani ticos OS 0coad eln( 0 ods 











(8-170) 
由 式 (8-170) 可 得 
Jute 
Ji2=cos0; (一 /2Sin0s +13 cos(O2 +63 ) ) 
J 13 =13cos01 sin(O2 +03) (8-171) 
Jiu=0 
Jis=0 
Jis =0 
类 似 地 ，Y6 和 Ze 的 导数 。 
= | =), res be Nee ee ey fe ee ee 


T 1 * Bey? 30; 


High, 十 sin01[ —l2sinO2 +l3cos(62 +43) 102 +13 sind) sin(02 +63) 03 








(8-172) 
OZ . a7 . a7 . . . . 
6 = it mL reseed ag 88a Oi tS a8. + +S 36 96 
Ei Teen a (8-173) 
由 式 (8-172) 和 式 (8-173) 可 得 
Jo=tu 
J22 =sinfı (— 12 sinO2 +13cos(G2 +83 )) 
J 13 =L; sinfı sin(O2 十 03 ) (8-174) 
J 24 =0 
J 25 =0 
J 26 =0 
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J31=0 

J 32 =l2cos02 +13 sin(O2 +83) 

J 13 =—13cos(02 十 03 ) (8-175) 
J314=0 

J35=0 

J36=0 


对 于 描述 末端 执行 器 坐标 系 绕 着 坐标 轴 旋 转 而 言 ， 没 有 确切 的 方程 。 因 此 ， 没 有 方程 通 
过 微分 求解 绕 3 个 坐标 轴 的 微分 转动 。 这 就 是 寻找 间接 的 或 者 更 加 系统 的 方法 估计 雅 可 比 和 矩 









































阵 的 原因 。 然 而 ， 通 过 计算 基于 角速度 矩阵 的 角速度 矢量 可 求 得 雅 可 比 和 矩阵 的 下 一 个 3 行 。 
0 —wz wy 
006 一 "ReoRIT 一 | wz 0 一 ox (8-176) 
—wy wx 0 
wx 
006 =| wy (8-177) 
wz 
这 时 重新 调整 分 量 以 显示 雅 可 比 和 矩阵 中 的 元 素 。 
_ 9wx Iw x ; | | Jw x ; 
wx 30, 1 20 se I; 6 (8-178) 
_ Iwy ¥ JOwy T 9wy 
wy 30 A; 39; Qaes 70. 06 (8-179) 
9 9 9 
oz 一 全 a ails (8-180) 














T 0, T 2 ee 76 
oo) oug oU6 
对 于 关节 机 械 手 ， 扩 展 式 (8-176) 表明 末端 执行 器 坐标 系 的 角速度 矢量 为 


wx—=sin0102 十 sin0103 十 cos01sin02304 十 (cos04sin01 一 cos01sin04cos023)05 十 








(cos@; cos@4 sinf23 十 Sin04 (sinfı sinf, cosO1cosO4 cos023 ))0s6 


(8-181) 
wy = — cos; 02 一 cos0103 十 Sin0l sinf23 64+ (—cosd) cosd4 — sinfı sin@4 cos823 ) 0 5 + 


[ — sin; cos@4 sin023 — sinf, (— cosd1 sin04 F sinfı cosOu cos23 ) 106 


(8-182) 





wz 一 01 一 cos(02 +63)64+sind, sin( 2 +05)05-+ {cost costs — 5 sindassin20, }d. 


(8-183) 
因此 ， 有 
Ja=0 
J 22 一 Sin01 
J 4 二 Sin0l 


J 44 = cosı (cos@2 sinf; 十 cosO3 sin0，) 
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J 45 =cosO4 sinO; 一 cosOl sinf, cos(02 十 03 ) 
Ji6 一 一 cosOlcosgusin(02 十 03 ) 一 sin04[Lsin0l sin04 十 cosOl cosd4cos(42 +63) ] (8-184) 


J51=0 
J 52=— cosh, 
J 53 = — cos 


J 54 = sinfı (cos@2 sin03 + cos; sind ) 
J 55 =— cos cosl, — sind; sinfscos(@2 +43 ) 
J 56 = —sin@; cos@4sin(@2 +03) 一 sin04[ 一 cosgl sin04 十 sin0lcosOucos(0 +43 ) | 



































(8-185) 
J6i=0 
J 62=0 
J 63 =0 
J 64 =—cos(O2 +43) (8-186) 
J 65 一 一 Sin04sin(02 十 03 ) 
J s6 = cosl, cos(02 十 03) 一 本 sin204sin(b， 十 03 ) 
KB 8-12 解析 式 雅 可 比 矩 阵 和 几何 式 雅 可 比 和 矩阵 。 
假设 未 端 执 行 器 坐标 系 的 全 局 位 置 和 方向 由 一 组 6 个 参数 确定 。 
or, 
X -(] (8-187) 
这 里 
oQ, =° $, Q) (8-188) 
式 (8-188) 是 3 个 独立 旋转 参数 ， 例 如 欧 拉 角 。 
Ora =° r, q ) (8-189) 
式 (8-189) 表示 末端 执行 器 坐标 系 的 笛 卡 儿 坐 标 位 置 ， 是 关节 变量 矢量 g 的 函数 。 
末端 执行 器 坐标 系 的 平 动 速度 为 
rn =g =J» (q )q (8-190) 
末端 执行 器 坐标 系 的 旋转 速度 为 
a Se : 
Pi Ft FOR (8-191) 


AZ, WERE WER ALOR ALF FARE fit, Fa RTE AT EERE Jo AEE HE yE E 
Jy 的 合成 在 形式 上 为 
H 
Ja = (8-192) 


式 (8-192) 被 称 为 解析 雅 可 比 和 矩阵 ， 说 明 它 与 几何 雅 可 比 矩 阵 是 不 同 的 。 


已 知 一 组 方向 角 $ ， 有 可 能 求 得 角速度 w 和 旋转 速度 之 间 的 关系 。 作 为 一 个 例子 ， 考 
JK 6. 3 节 中 所 定义 的 绕 zxz 轴 的 欧 拉 角 bp 。 依 据 欧 拉 频率 ， 全 局 角速度 在 式 (2-130) 中 
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求 得 。 
WX 0 cosp singsing \I? 
四 = | sing cos 9 (8-193) 
WZ 1 0 coOSO 
4 
o 一 “RE 由 (8-194) 


欧 拉 频 率 p、0 、yY 是 关节 速度 的 函数 。 


9 


0|=J, gq (8-195) 
y 
因此 ， 有 
Jr=°REJ, (8-196) 
:= (8-197) 
JR 
当 末 端 执行 器 的 角速度 以 笛 卡 儿 频 率 表 述 为 
Wx 
w=| wy (8-198) 
WZ 








DER, HE AT HEIE REIER J JUL JERI EEE (8-197)。 当 末端 执行 器 的 角速度 以 非 笛 卡 儿 角 频 
率 形式 表述 如 欧 拉 表述 ， 那 么 雅 可 比 矩 阵 被 称 为 解析 雅 可 比 和 矩阵 (8-192)。 








8.4 逆 问 速度 运动 学 


逆向 速度 运动 学 问题 ， 也 就 是 众所周知 的 速率 分 解 问题 ， 主 要 是 寻找 与 末端 执行 器 速度 
矢量 多 相符 的 关节 速度 矢量 qg。6 个 自由 度 能 够 在 任意 方向 上 以 任意 角速度 移动 末端 执行 器 。 
末端 执行 器 速度 矢量 忆 通 过 雅 可 比 矩 阵 J 与 关节 速度 矢量 9 相关 。 


E oo, Jp P R 
x= =| |q=Jq (8-199) 
O, JR 


因此 ， 对 于 逆向 速度 运动 学 ,我们 要 求 依据 末端 执行 器 的 笛 卡 儿 平 动 速度 和 角速度 在 关 

节 坐 标 系 中 的 表述 进行 微分 变换 。 如 果 雅 可 比 和 矩阵 在 所 计算 的 某 一 时 刻 是 非 奇异 的 ， 雅 可 比 
和 矩阵 的 逆 德 阵 ! 是 存在 的 ， 我 们 便 能 够 求 得 所 要 求 的 关节 速度 矢量 : 
q =J X (8-200) 

F E HE TE REY 7 RE. A j: 是 不 能 确定 的 。 式 (8-200) 确定 


在 每 个 关节 人 处 所 要 求 的 速度 ， 以 便 形成 所 期 望 的 末端 执行 器 速度 处。 
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因为 逆向 速度 运动 是 正 向 速度 的 结果 ， 需 要 求 矩 阵 的 首 和 矩阵 ， 这 个 问题 相当 于 一 组 线性 
代数 方程 的 解 。 为 了 求解 [三 TAMAHAE RÉA 

例 8-13 平面 极 坐 标 机 械 手 的 逆向 速度 。 

8-2 所 示 为 一 个 具有 下 列 正 向 速度 方程 的 平面 极 坐标 机 械 手 。 



































x cosSO —rsind : 
alle 9 Pale (8-201) 
y sin rcos b 
为 了 确定 逆向 速度 ， RITA A EAE YE EA 
OX 3X 
or 90 cos? —rsind 
J= =|. (8-202) 
IY IY sind recos 
or ag 
ay ax 
jos 1 90 90 | 1 | cosé „l TETN 
9X OY AX oY| OY aX r\l—sin@ cos 








or 0g ag or or or 


r 1 | COSO | X 
Sa ] (8-204) 
4 rl—sin@ cosé ý 
例 8-14 2R 平面 机 械 手 的 逆向 速度 。 


例 5-8 和 例 6-3 已 经 分 析 了 2R 平面 机 械 手 的 正 向 和 逆向 运动 学 。 其 雅 可 比 和 矩阵 和 正 向 
速度 运动 在 例 8-5 中 已 进行 了 求解 ， 即 有 


因此 ， 有 








X =Jq (8-205) 
x 一 /lsingl —l2sin(@: +62) —l2sin(@1+02)\{ Ar 
= (8-206) 
y 1, cos@; +12cos(@, +02) lz cos(01ı +02) A> 
Di [ay E EE zs OF es BE AE AT ECE AE ME. Ak. A 
q=-J 'X (8-207) 
6, —lssin0; —lesin(@; +02) 一 02Sin(O +02) =1 X 
= . (8-208) 
A, l2 cos +12cos(O, +02) lzcos(01 +02) Y 
—] 一 /2cos(O1 +02) —losin(O, +02) 
J= Eca | (8-209) 
lil2sin02 \1icosO1l2cos(01 二 02) Lisin01 十 sin(O1 十 02) 
因此 ， 有 
，  Xcos(@; +62) +Ysin(6; +0 
六 二 cos(01 2) sin(01 2) (8-210) 


Z, sind» 
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2 [iieosb 十 /zcos(O1 oe eae abe sind; +lssin(@; +82) | 
2 
例 8-15 2R 机 械 手 的 奇异 配置 。 
当 雅 可 比 和 矩阵 [ 式 〈8-206)] 的 行列 式 为 零 时 ，2R 机 械 手 的 奇异 性 便 会 出 现 。 从 例 
8-14 中 我 们 得 知 





(8-211) 


—I1,losin@2 





(一 sin0l 一 02sin(O1 十 02) 一 02sSin(O +02) 
-| li1cos01 二 12cos(01 二 0,) Zycos(@; +82) ree 
J NAT 
|J | =L l2sin0? (8-213) 
因此 ， 机 械 手 的 奇异 配置 为 
A, =0°, 02 =180° (8-214) 





式 (8-214) 分 别 对 应 完全 伸 出 或 者 全 部 紧缩 配置 ， 如 图 8-7a 和 图 8-7b 所 示 。 在 奇异 配置 处 ， 
Oi 的 值 是 不 确定 的 ， 可 以 是 任何 实数 值 。 雅 可 比 和 矩阵 的 两 列 相似 ， 因 为 式 (8-206) 变 成 了 


X 一 Sin01 \ ， 一 Sin01 \- 。 : 一 Sin01 
=21, A, +12 9 0 一 (20101 十 /20，) (8-215) 


. cost cost cost 




















图 8-7 2R 平面 机 械 手 奇异 配置 
a) 全 部 伸 出 b 全 部 紧缩 





在 这 种 情况 下 ， 端 点 仅仅 只 能 在 垂直 于 机 械 辟 连 杆 的 方向 上 移动 。 

例 8-16 道 速 度 运动 学 的 分 析 方 法 。 

理论 上 ， 我 们 必须 能 够 从 描述 正 向 速度 方程 中 计算 关节 速度 ， 然 而 这 样 的 计算 在 通常 情 
况 下 是 不 太 容 易 的 。 

作为 一 个 例子 ， 考 虑 2R 平 面 机 械 手 ， 如 图 8-3 所 示 。2R 机 械 手 的 端点 速度 可 以 被 表述 
在 式 (8-105) 之 中 ， 即 


odz =6 1% ko X (L1? i1 12% ig) +0201 XL2?i? (8-216) 
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该 式 与 i， 进 进行 点 H, 则 有 


"d: 0i =01 Co X 1191) °i2=11 1° ko 7 Ci] Xli) =1101%Ro 


因此 ， 有 
də a 07, 


i : = 
li sinl? 


式 (8-216) Giya 进行 点 积 ， 则 有 


Ody +0 i =) CRo XLi) +Â CÊ Èi XLl?i2)?i] 


=, (01 +02) Êo z Ciz Xi )=— Ll: (0, +62) sinds 


因此 ， 有 
og 03] 


L2 sinĝ2 





02 二 一 01 


当 末端 点 速度 "ds 二 [ 义 ”Y] 给 定时 ， 我 们 可 以 确定 关节 速度 91 M Od. 
ree SLA BRIG Fey WLI A. HEET HAE Me GEE BE 
具有 球形 手腕 的 关节 机 器 人 ， 例 8-9 中 计算 了 其 雅 可 比 矩 阵 为 






































j= oko X? de 081 X? de of, XY ds 0 
oo of, oÈ, Of, 





RATHER LL REA oP a 4 3&3 TEE, RE R: 


He SHE 





a 
HE 


0 


Ry 
JAE 3X3 子 和 矩阵 为 零 。 这 是 因为 球形 手腕 的 结构 以 及 其 最 后 3 个 位 置 矢 置 为 零 。 


A=(%% xX) ds ok X? ds of, XY ds) 


C= (ko Of, ok ,) 
D=(k; Ob, oks) 





2 0 


0 


0ks 


| 














72s8in@2 =/) 6, sinz 


(8-217) 


(8-218) 


(8-219) 


(8-220) 


(8-221) 


(8-222) 


(8-223) 
(8-224) 
(8-225) 


如 果 我 们 利用 B=0, ABA ic RE AYE AT FEAE BAe RI D eS fe T BL aie ACB) IE Ty E BE es 








动 学 可 写成 : 


X=Jq 








(8-226) 


(8-227) 
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式 (8-227) 的 上 半 部 分 为 








0 
od: =A 0， (8-228) 
0; 
式 (8-228) 可 被 反 求 ， 即 
0 
0， =A-1 gd; (8-229) 
0; 
IN (8-227) 的 下 半 部 分 为 
01 
2 . l 
: 01 6, 
03 ; : 
002: =(C D) _ |=C A» +D as (8-230) 
64 l 
l 03 06 
05 
T 
因此 ， 有 
ô, ay 
6; |=D-|o@2—C} 9, || =D! (ow, -CA odz) (8-231) 
Os 0; 


例 8-18 ”关节 式 机 器 人 的 逆 速 度 。 
8-4 中 关节 式 机 械 手 的 端点 在 例 8-7 中 可 被 求 得 ， 位 置 矢量 为 


Xp cos0O1[l2cos02 +3 sin(62 +43) | 











Yp sind, [22 cosô2 +13 sin(@, +43) | 
orp = = (8-232) 
Zp lı —l3cos(@2 +63) +l2sind2 


ra 
j= 


利用 矢量 "rp 的 分 量 ,， 我们 可 以 计算 机 械 手 的 雅 可 比 和 矩阵 。 
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aXp IXp exe 
a6, 30， 903 
aYp ayp ayp 
36, 30， 90; 
Zp Zp B75 
a6, 90» 903 

















以 便 求解 机 械 手 的 正 向 运动 。 


Xp 6, 
Yp |=J|9, 
Zp 03 


为 了 计算 机 械 手 的 逆 速 度 运 动 学 和 矩阵， 我们 需要 计算 三 。 
0OXP IXp 9Xpy-l 
901 302 903 
J = ove ve oY p =|a21 422 423 
901 302 203 p ~ 5 
IZ p IZ p IZ p 
901 302 20; 
sind; 


~ Jasint hy | 03) t lacob; 




















U1l 


cos01 





1 
an=— 7 (sine T0s)) 


coSO3 


1 cosi 








(23 sin(@2 +63) +l2cos@2 ) 


dis 
”™ Lol3 cos03 


7 cos 
A La sin(02 +03) +l2cos@2 
1 sin(C02 十 03 ) 





al 








oe l2 COSO3 an 
sinfı ; 

a32 => (lz sin( 82 +43) +l2cosd2) 

Lə l3 coss 
aiz; = 0 

_ cos(02 +43) 

saim l2 coss 
a33 = (13cos(0» 十 03 )—L2sind2 ) 


1213cos0; 





因此 ， 机 械 手 的 关节 速度 01. 02. 05.8% 


01 Xp 
02 =e Yp 
03 Zp 


(8-233) 


(8-234) 


(8-235) 


(8-236) 


(8-237) 


(8-238) 


(8-239) 
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8.5 本 全 小 结 





串 行 机 器 人 中 的 每 根 连 杆 都 有 一 个 角速度 和 平 动 速度 。 通 过 其 下 阶 连 杆 全 局 角速度 之 和 
可 求 得 连 杆 (i) 全 局 角速度 为 








Jo 一 >) ;0; (8-240) 


j=l 
使 用 D-H 参数 ， 相 对 于 连 杆 (j 一 1) 的 连 杆 G) 角速度 为 




















A of. x y e 
e E a (8-241) 
0 (对 于 了 关节) 
JEM (1) 的 平 动 速度 是 附着 在 连 杆 G) 上 的 坐标 系 B: 原点 的 全 局 速度 ， 即 
. 00; X ; 1 di (对 于 R 关节 ) 
a ae (8-242) 
d:? kii t9 @; X;1di OFF PRD 
这 里 ，0 Ald 是 D-H 参数 ，d 是 坐标 原点 的 位 置 矢量 。 
通过 关节 速度 g 和 来 端 执 行 器 全 局 速度 X 之 间 的 关系 定义 机 器 人 速度 运动 学 。 
q =(g, qn qn oe qn)T (8-243) 
K(X, Y, Zs Wx, wY; wz, )T (8-244) 
这 样 的 关系 引入 雅 可 比 和 矩阵 J 
X=Jq (8-245) 


已 知 了 了 ， 对 一 组 给 定 的 关节 速度 ， 我 们 可 以 求 得 未 端 执 行 器 速度 ， 反 之 亦 然 。 雅 可 比 矩 
阵 是 关节 坐标 的 函数 ， 是 机 器 人 速度 运动 学 中 的 主要 工具 。 
实际 上 ， 由 ci (qd 所 表示 的 雅 可 比 生 成 拓 量 便 可 计算 雅 可 比 和 矩阵 J。 



































ok 1 X,Yd, 
ci (Cg )= . (8-246) 
Oki] 
XE, cq) 形成 了 雅 可 比 和 矩阵 中 的 ;一 1 列 。 
JJ 一 (co cl C2 … C1) (8-247) 
习题 
8-1 标记 与 符号 。 
FIR PUGET SIN 
1) ild; 2); 0d; 3) ,id.; 4) ldi 5) ild; 6) bdi 
Dild 8) ?Yd Did 10 jdi 1D ld 12 Vd; 
13) °R; 14) °k; 15) ; ok; 16) ; lv; 17) ;-jv; 18) iv; 





8-2 3R 平 面 机械 手 速度 运动 学 。 
5-21 所 示 为 一 个 有 1 RIR 平 面 机 械 手 。 机 械 手 的 正 向 运动 提供 了 下 列 变 换 矩 阵 ; 
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c03 —sO3 0 /3c03 cO 一 SO 0 Loc0， 
SO3 cO3 0 /3SO03 s02 c02 0 /2»s0» 
2T; = 1T, = 
0 0 1 0 0 0 0 
0 0 0 1 0 0 0 il 
cO, 一 SO01 0 Lich) 
sO, cA, 0 Ll1s01 
oT 一 
0 0 1 0 
0 0 0 1 
针对 以 下 数值 : 
0 一 560，0 = — 28°, 8; =— 10°, lı =100cem, lz =55cm, la 一 30cm，01 一 307]/s， 


As, Gs =— 10s; 
AH EL Fe AY HET EERE J, H BOR BAS FILEE R E 
8-3 ”球形 手腕 速度 运动 学 。 








5-26 所 示 为 球形 手腕 的 原理 。 下 面 给 出 了 相关 的 变换 矩阵 ， 假 设 坐 标 系 Bs 是 基体 坐 
标 系 ， 试 求 坐标 系 Be 的 角速度 矢量 。 
c04 0 一 SO4 0 cs 0 SO5 0 
, sO, 0 cO4 0 sôs 0 一 c05 0 
`T, 一 1T; 一 
0 =i 0 0 0 1 0 0 
0 0 0 1 0 0 0 1 
chs 一 SO6 0 0 
is sOe cog 0 0 
”Te 
0 0 1 0 
0 0 0 1 


8-4 ”球形 手腕 和 手 爪 坐标 系 速度 运动 学 。 
假设 我 们 将 具有 下 列 变换 矩阵 的 手 爪 坐标 系 添 加 到 球形 手腕 的 最 后 一 个 坐标 系 Be 中 。 


1 0 0 0 

0 1 0 0 
6T; = 

0 0 1 ds 

0 0 0 1 





手腕 变换 矩阵 在 习题 8-3 中 已 给 出 。 假 设 坐 标 系 Bs 是 基体 坐标 系 ， 试 求 手 爪 坐标 系 Br 
的 移动 速度 和 角速度 。 

8-5 SCARA 机 械 手 速度 运动 学 。 

一 个 具有 下 列 变换 矩阵 的 RT RI R || P SCARA 机 械 手 如 图 5-23 所 示 。 利 用 雅 可 比 生 


成 矢量 技术 计算 雅 可 比 和 矩阵 。 
coO1 一 SO 0 ic c03 —s03 0 0 
oT, = SO1 cO1 0 lis) T s03 cs 0 0 
0 0 1 0 0 0 1 0 
0 0 0 1 0 0 0 1 


327 


ws: 运动 学 、 动 力学 与 控制 技术 





c02 一 SO， 0 l2 c02 1 0 0 0 

s02 c02 0 /2»s0» 0 1 0 0 
1T, = 3ST, 一 

0 0 1 0 0 0 1 d 

0 0 0 1 0 0 0 1 


8-6 RFR || R 关 节 式 机 械 臂 速度 运动 学 。 
5-22 所 示 为 一 个 具有 下 列 变换 矩阵 的 3 AAEH RER || R 机 械 手 。 利 用 直接 微分 和 
雅 可 比 生成 矢量 法 求 得 雅 可 比 矩 阵 。 








cı 0 一 SO1 0 cO3 0 SO3 0 
01 0 cO1 0 SO3 0 — ch; 0 
oT 一 2 人 3 一 
= 0 dı 0 1 0 0 
0 0 0 1 0 0 0 1 
c02 — s> 0 1202 0 
sO 2 c02 0 ¿2s02 
1T, = orp =° Ts 
0 0 1 0 3 
0 0 0 1 1 





8-7 RRP 平面 元 余 机 械 手 。 











Al 8-8 RRP MICA HL HF 








8-8 所 示 为 一 个 具有 关节 变量 9 、0。 和 ds 的 3 自由 度 平 面 机 械 手 。 

D 确定 连 杆 变换 矩阵 ， 计 算 "T3 。 

2) 对 于 给 定 值 X、Y、9? 求解 机 械 手 的 逆向 运动 学 ， 其 中 X, Y 是 未 端 执行 器 坐标 系 
By 的 全 局 坐标 ，9p 是 坐标 系 Bs 的 角 坐标 。 

3) 证 明 ， 下 列 方程 是 着 向 运动 问题 的 一 组 解 。 














B JX2+Y2 | 
02 =arctan = sin(d2—a) 
+ 1—8? li 
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学 问题 。 


d2, 


手 的 逆向 运动 学 ， 其 中 X、Y 是 末端 执 
fri bts AB; 的 全 局 坐标 ，9 是 坐标 系 
Bs 的 角 坐 标 。 


正 向 速度 运动 学 问题 





Y 
0, =arctan (@2—a) a=g-—arctan 


X X 





d3=a/12 +X? +Y? —2lı JVX? FY? cos(0 —a) 
4) 确定 机 械 手 的 雅 可 比 和 矩阵 ， 并 且 证 明 下 列 方程 可 以 求解 正 向 速度 运动 学 问题 。 








x 01 
ys 
p d3 


—1,s0,—d3s(6, +02) —ds(01 +02) c(01 +02) 
J= —1,c0; +d3c(O; +42) d3c(@, +082) s(01ı +02) 
1 1 0 


5) 确定 J :并 且 求 解 逆向 速度 运动 








8-8 RPR 平面 元 余 机 械 手 。 
图 8-9 给 出 了 具有 关节 变量 的 01、 
02 的 3 自由 度 平面 机 械 手 。 
1) 确定 连 杆 变换 和 矩阵， 计算 "Ts。 
2) 对 于 给 定 值 XxX、Y、o 求解 机 械 











3) 确定 机 械 手 的 雅 可 比 矩 阵 以 求解 





图 8-9 RPR 平面 元 余 机 械 手 


x A, 
Y |=J] 9， 
ds 


9 
4) 确定 J 并且 求 解 逆向 速度 运动 学 问题 。 
8-9 ”补偿 关节 式 机 械 手 。 
8-10 所 示 为 一 个 补偿 关 方式 机 械 手 。 
1) 确定 机 械 手 的 正 向 运动 学 。 
2) 确定 末梢 点 卫 的 全 局 坐标 。 
3) 利用 直接 微分 确定 机 械 手 的 雅 可 比 矩 阵 。 
4) 利用 雅克 比 生成 矢量 法 确定 机 械 手 的 雅 可 比 和 矩阵 。 
5) 确定 雅 可 比 逆 和 矩阵 以 求解 逆向 速度 运动 学 。 
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图 8-10 ”补偿 关节 式 机 械 手 


8-10 SCARA 机 器 人 逆向 速度 运动 学 。 

8-11 所 示 为 一 个 SCARA 机 器 人 。 

1) 确定 坐标 系 By 原点 在 全 局 坐标 系 G (G=By) 中 的 坐标 。 
2) 利用 直接 微分 确定 机 械 手 雅 可 比 和 矩阵 。 

3) 利用 生成 矢量 确定 机 械 手 雅 可 比 矩 阵 。 

4) 确定 雅 可 比 矩 阵 的 逆 矩 阵 以 求解 逆向 速度 运动 学 。 

















图 8-11 SCARA 机 器 人 


友 8-11 地 面 上 具有 坐标 系 Bo 的 SCARA 机 器 人 。 

8-12 所 示 为 一 个 地 面 上 具有 坐标 系 Bo 的 SCARA 机 器 人 。 
1) 确定 坐标 系 B 原点 在 全 局 坐标 系 G (G=Bo) 中 的 坐标 。 
2) 利用 直接 微分 确定 机 械 手 雅 可 比 和 矩阵 。 
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3) 利用 雅克 比 生成 矢量 确定 机 械 手 雅 可 比 和 矩阵 。 
4) 确定 雅 可 比 和 矩阵 的 逆 和 矩阵 以 求解 逆向 速度 运动 学 。 


























图 8-12 地面 上 具有 坐标 系 Bo 的 SCARA 机 器 人 





8-12 刚体 连 杆 速度 。 
图 8-13 所 示 为 坐标 系 和 刚性 连 杆 G) 的 速度 矢量 。 试 回答 以 下 问题 。 

















图 8-13 刚性 连 杆 速度 矢量 


D 依据 d ;和 d;_1 确 定 连 杆 在 质心 Ci 处 的 速度 vv;。 


2) 依据 d Md: 确定 连 杆 的 角速度 va， 。 

3) 依据 近 端 关节 i 的 速度 确定 连 杆 在 质心 C; 处 的 速度 "ui 。 
D 依据 远 端 关节 i 十 1 的 速度 确定 连 杆 在 质心 Ci 处 的 速度 "wi; 。 
5) 依据 远 端 关节 i 十 1 的 速度 确定 近 端 关节 i 的 速度 。 

O 依据 近 端 关节 i 的 速度 确定 远 端 关节 i 十 1 的 速度 。 
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友 8-13 PRRR 机 械 手 的 雅 可 比 和 矩阵 

对 于 图 6-14 中 所 示 的 机 械 手 ， 确 定 其 雅 可比 和 矩阵 。 

W814 球形 机 器 人 速度 运动 学 。 

配 有 球形 关节 的 一 个 球形 机 械 手 R 上 FR 上 P， 如 图 5-43 所 示 。 该 机 器 人 的 变换 和 矩阵 在 例 
5-36 中 已 经 给 出 ， 试 求 该 机 器 人 的 雅 可 比 矩 阵 。 

友 8-15 空间 站 远程 遥控 机 械 手 系统 的 速度 运动 学 。 

对 于 图 5-24 中 所 示 的 航天 飞机 遥控 机 械 手 系统 来 说 ， 其 变换 矩阵 在 例 5-26 中 已 经 给 
出 。 通 过 计算 雅 可 比 抢 阵 求解 航天 飞机 遥控 机 械 手 系统 的 速度 运动 学 。 




















332 


运动 学 中 的 数值 法 





随 着 连 杆 数目 的 增加 ， 机 器 人 学 中 的 解析 计算 将 变 成 一 项 乏味 的 任务 ， 需 要 数值 计算 。 
下 面 我 们 回顾 机 器 人 学 中 使 用 最 频繁 的 数值 分 析 法 。 


9.1 线性 代数 方程 


在 机 器 人 分 析 中 ， 存 在 着 一 些 问 题 和 情况 ， 如 逆向 运动 学 ， 我 们 需要 求解 一 组 耦合 
线性 或 非 线性 代数 方程 。 求 解 非 线性 方程 的 每 种 数值 方法 都 要 通过 迭代 求解 一 组 线性 








方程 。 
考虑 具有 常 系数 的 7 个 线性 代数 方程 的 系统 。 
44,41 aig T2 cr A), Ln VI 
Ay LITE gn E2 T Ta Ln =b? 
Ay F1 Taz, %2 eTA pnn =b, (9-1) 














式 (9-1) teh AY iy RIE REJE I: 
Ax=b (9-2) 
有 很 多 方法 可 求解 这 组 方程 。 最 有 效率 的 方法 就 是 LU 因数 分 解法 。 
对 于 每 个 非 奇 异 和 矩阵 4 ， 存 在 一 个 具有 非 零 对 角 元 素 的 上 三 角 和 矩阵 和 具有 单位 对 角 元 
素 的 下 三 角 和 矩阵 L， 即 


A=LU (9-3) 
Tii. G12 Qin 
a a oe a 
A= 21 22 2n (9-4) 
any an2 a an 
1 0 0 
Lo 1 eee 0 
L= (9-5) 
lal la 1 
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Ull u12 U ln 
O uz è ** U2n 
U= (9-6) 


0 0 eee Unn 
将 矩阵 A 分 解 成 LU 的 过 程 被 称 为 LU ARM. — HIREL AE PU FE, R (9-2) 


LUx=b (9-7) 
将 式 (9-7) 变换 成 式 (9-8) MIÈ (9-9) 即 可 求解 。 

Ly=b (9-8) 

Ux=y (9-9) 





IN (9-8) 和 式 (9-9) 都 是 三 角形 方程 组 ， 它 们 的 解 容易 通过 正 向 和 反 向 替代 获得 。 
证 明 : 
为 了 说 明和 抑 阵 4 怎样 被 分 解 成 上 U， 我 们 考虑 一 个 4X4 乞 阵 。 





人 1 0 O Ofunu wiz wis un 

Qo, Q3 Qə Any lx 4q 0 O}} O uz uz uz 
= (9-10) 

G3, Axo Az, Gay L31 L32 1 0 0 0 U33 U34 

lu laz Lla 1 0 0 O us 


ayj ajz das Qk 
利用 伪 和 矩阵 B， 我 们 可 以 将 下 三 角 和 矩阵 L 和 上 三 角 和 矩 阵 U 中 的 元 素 合成 为 伪 和 矩阵 B 。 


Uil Uiz? U1l3 un 





l21 uz U23 U24 
B= (9-11) 
L31 L32 U33 u34 
lua la Ll3 un 
fi RAP TT AI. —A— A EEB PR. 
d) (2) (3) 4 
(5) (8) (9) (10) 
B= (9-12) 
(6) (11) (13) (14) 
(7) (12) (15) (16) 
对 于 生成 与 n Xn 和 矩阵 A 及 其 相 联 系 的 矩阵 史 的 过 程 ， 在 7 一 1 次 大 代 内 完成 。 在 ;一 1 次 
迭代 之 后 ， 和 矩阵 的 形式 为 

















Ul Ua, UN Ul 
la,l Ua,2 Wn 
B= Ui-1sn (9-13) 
三 q 
Dp 
basa ‘lass ieee - 4 








右 下 角 中 未 被 处 理 的 (2 —i +1) X nit) FEMA SD trem. ECA eh 
具有 相同 的 元 素 。 在 第 ; 步 ，LU 因数 分 解法 将 D ;分解 为 
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dii ri 
D;= (9-14) 
Si Hi+i 
转换 成 一 个 新 的 形式 : 
U ii ul 
D;= (9-15) 
L; Di+1 
直接 相 乘 ， 则 有 
U11 二 411 U12 5A] U13 SA ]3 U4 = Ay, (9-16) 
a a a 
la =, la =— la =— (9-17) 
M11 Ull uli 
U22 =d p —L 21 W12 »U23 =A, —L 21 U13 U24 =Ay, —Laiuis (9-18) 
Gy, —l31 U2 Gy 一 241U12 
La 一- Le (9-19) 
U22 U22 
U33—=ass— (13113 +232 23) U3 a — (U1un4 tl32u24) (9-20) 
Qs3— (laui Flazuzg) 
143 = (9-21) 
U33 
us 一 Qi 一 (4 十 042224 十 243234) (9-22) 
因此 ， 对 于 获得 L 和 U 与 n Xn 系数 矩阵 4 相当 的 元 素 ， 其 常用 公式 可 被 写成 
i—i 
Uij =a, 一 D larur < dl Wy (9-23) 
k=l 
j-l 
aij —= X lrun 
li = am iLi i=l (9-24) 
Ujj 


XJF i=l, u 的 规则 可 简化 为 


Ulj aij (9-25) 
对 于 J 二 1, 2 的 规则 可 简化 为 
ail 
Lii =— (9-26) 
ull 
TAFE RE BIC k) 的 计算 仅仅 只 涉及 处 于 同一 位 置 处 矩阵 A 中 的 元 素 ， 以 及 和 矩阵 B 中 的 





一 些 已 被 计算 的 元 素 。 
在 一 个 算法 中 ， 建 立 LU 因数 分 解 技术 以 便 容易 进行 数值 计算 。 
算法 9-1 对 于 nXn MR. H LU 因数 分 解 技 术 如 下 : 
1) 设置 初始 计数 值 ;一 1。 
2) RAD =A., 
3 依据 如 下 步骤 从 D; 计算 D;+i。 





ui; =d ij (9-27) 
eg au (9-28) 

1 
l; =—s; (9-29) 

U ii 
Disi1=Hi4i lu; (9-30) 
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4) 设置 i 二 i 十 1]。 如 果 i 一 n， 那么 LU 因数 分 解 完成 ， 否 则 返回 至 第 3) 步 。 
将 矩阵 A 分解 成 矩阵 L 和 U 之 后 ， 用 下 面 的 算法 可 以 求解 这 组 方程 。 
算法 9-2 LU 求解 技术 如 下 : 
1) 通过 如 下 步骤 从 Ly 一 b 中 计算 y。 
y1 一 01 














y2 一 02 一 y1721 
y3 王 03 一 y1731 一 y2732 


inl 
yi bi — Dy ili 
j=l 
2) 通过 如 下 步骤 从 Ux=y 中 计算 x。 

















PE Yn 
ot 
Nn,n 
— Yn-1 TnUn—l,n 
Tn—l 
Un—l,n—1 
1 n 
Xi gr (v: — 5 Xjui ) 
可 j=itl 
例 9-1 求解 4 个 方程 的 方程 组 。 
考虑 如 下 方程 : 
Ax=b 
这 里 
2 1 3 =3 
1, o = 1, =2 
A= 
0 2 2 1 
3 l 0 =z 
1 
2 
b= 
0 
—2 
按照 LU 因数 分 解 算 法 ， 第 1 步 我 们 设置 : 
i=l, D,=A 
进而 求 得 
dii=2, r= 3 一 3) 
1 O =]. 2 
si H:=|2 1 
íl =A 
则 有 
u1 =d =2 
1/2 
ul =rli=(] 3 3l =—s =| 0 
3/2 
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(9-31) 


(9-32) 


(9-33) 


(9-34) 


(9-35) 


(9-36) 


(9-37) 


(9-38) 


(9-39) 


(9-40) 


= zazpina 


a 
Ne) 








D:=H:—lu =| 2 2 1 (9-41) 


第 2 步 我 们 设置 : 




















i=2 (9-42) 
求 得 
d Ep L (9-43) 
22 2 "2 2 2 
2 1 
s=| 1 | H3= (9-44) 
一 二 一 9/2 5/2 
2 
计算 则 有 
1 
U22 二 d 22 二 一 一 (9-45) 
2 
5 1 1 一 4 
T= = = == Cx 
Us —r > | 3 =) I, ns? [| (9-46) 
一 8 一 1 
D3;=H3—lou3 -| | (9-47) 
一 2 3 
第 3 步 我 们 设置 ; 
1 一 3 (9-48) 
求 得 
d33=—8 r; 一 (一 1) (9-49) 
S3 一 (一 2) Hy=(3) (9-50) 
因此 ， 有 
u33 =d33 =— 8 (9-51) 
P 1 1 
Us =r; 一 (人 1) l; =— s; 一 FA (9-52) 
U 33 4 
Di 一 下 一 5 一 [时 | (9-53) 
在 这 些 计 算 之 后 ， 和 矩阵 下 、 世 各 变 为 
2 1 3 一 3 
1/2 1/2 5/2 1/2 
B= (9-54) 
0 4 一 8 一 1 
3/2 1 1/4 13/4 
1 0 0 0 
1/2 1 0 0 
L= (9-55) 
0 —4 1 0 
3/2 1 1/4 1 
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2 1 3 =3 
y=|0 T12 一 5/2 一 1/2 GH 
0 0 一 8 = 
0 0 0 13/4 
现在 求解 矢量 y 以 满足 : 
Ly=b (9-57) 
I 
3/2 
y= g (9-58) 
=13/2 
最 终 应 该 求 得 未 知 矢 量 x ， 以 满足 : 
Ux =y (9-59) 
—5/2 
x= ne (9-60) 
—2 


例 9-2 具有 支点 的 LU 分 解 。 

在 LU 分 解 过 程 中 ，xz =0 会 带 来 一 个 除 以 零 的 情况 ， 这 是 不 可 避免 的 。 在 这 种 情况 
下 ， 必 须 应 用 支点 。 通 过 支点 ， 我 们 必须 改变 方程 的 顺序 以 获得 一 个 具有 最 大 元 素 的 系数 和 矩 
阵 。 在 绝对 值 上 ， 这 些 最 大 元 素 作为 对 角 线 元 素 。 最 大 的 元 素 被 称 为 支点 元 素 。 















































Ax =b (9-61) 
2 1 3 =3 ye 1 
1 0 =1 2 x2 2 
= (9-62) 
0 2 0 1 |j zs 0 
3 1 4 — 2j (z4 =? 
然而 ， 先 互 换 第 1 行 和 第 4 行 ， 再 互 换 第 1 列 和 第 3 列 ， 我 们 将 最 大 元 素 移 至 di 。 
3 1 4 = A 
1 0 =] 2 x2 2 
= (9-63) 
0 2 0 1 X3 0 
2 1 3 — 3) (z4 1 
4 1 3 —2)\(23 —2 
= 0] 2 X? 2 
= (9-64) 
0 2 0 1 Xl 0 
3 1 2 —3/\z4 1 
TEA PAI 3X3 子 和 矩阵 中 ， 最 大 元 素 将 移 至 dz. 
4 —2 3 1)\(x3 — 7, 
—] 2 1 Of}axa 2 
= (9-65) 
0 0 2||zi 0 
3 一 3 2 1)\axe I 
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第 


9 


= zazpia 


























4 2 3 Nz) ee 
3 3 2 1lz |1 
0 i © li | 
=i > 1 Ole 2 

最 后 ， 在 右 下 角 的 2X2 矩阵 中 ， 最 大 的 元 素 将 移 至 dzz. 
4 —2 1 3 /—2 
3 —3 1 2llzs| |1 
o 1 2ollz |0 
=i 工作 二 2 





(9-66) 


(9-67) 


为 了 应 用 LU 因数 分 解 算 法 和 LU 求解 算法 ， 我 们 定义 如 下 新 的 方程 : 


A'x'=b' 
4 —2 1 3)\(2x} 一 2 
3 = 工艺 2 | 
0 1 2 0|| 0 
一 ] 2 0 l)hei 





基于 LU 因数 分 解 算 法 ， 第 1 步 我 们 设置 : 
i=] 











并 且 知 道 
Di=A’ du=4 rl=(—2 1 3) 
3 一 3 1 
si=| 0 H:=| 1 2 
一 1 2 0 1 
计算 得 
2 一 di 一 4 ui=ri=(—2 1 3) 
3/4 
h=] 0 
uj) 
—1/4 
一 3/2 1/4 —1/4 
D.=H2,—lui=| 1 2 0 


第 2 步 我 们 设置 : 








计算 得 





(9-68) 


(9-69) 


(9-70) 


(9-71) 


(9-72) 


(9-73) 


(9-74) 


(9-75) 


(9-76) 


(9-77) 


(9-78) 


(9-79) 
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"=(3 7 1 | (9-80) 
13/6 —1/6 
D= -bu =| si | (9-81) 
第 3 步 我 们 设置 : 
i=3 (9-82) 
并 且 知 道 
dn = 一 [一 | (9-83) 
i on 
计算 得 
u33 =d 33 A ul r3 | 3 (9-85) 
n= s=(Ż) (9-86) 
Di =H. -u}= (2) (9-87) 
13 
FAK. SB BL 和 U 为 
1 0 0 0 
en vog (9-88) 
0 —2/3 1 0 
—1/4 —1 3/13 1 
4 —2 1 3 
本 0 一 3/2 1/4 —1/4 
0 0 13/6 —1/6 
0 0 0 20/13 
现在 ， 我 们 能 够 求 得 矢量 ?为 
Ly =b' (9-90) 
一 2 
J n (9-91) 
47/13 
计算 未 知 矢量 x 为 
Ux' =y (9-92) 
zi —69/29\) (x3 
; zh —19/10 XA 
sad bale ee (9-93) 
zi 47/20 X1 
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因此 ， 有 
Xx 47/20 
19/20 
e ine Nad (9-94) 


KG 9-3 解 的 唯一 性 。 
考虑 一 组 具有 个 线性 方程 的 方程 组 Ax 一 b。 如 果 和 矩阵 A 是 方 阵 并 且 是 非 奇 异 的 ， 那 么 
存在 着 唯一 解 xz 一 4 了 功 。 然 而 ， 如 果 方 程 所 代表 的 线性 系统 涉及 了 ?2 个 变量 和 wm 个 方程 。 






























































äiti Täta T ainn =b 
Ay T e aaa Xn =b2 

aia ll eee = = 
anit 17 A m2 H2 A mnn bm (9 95) 








这 时 ， 可 能 有 三 类 解 。 

1) 存在 唯一 解 并 且 系 统 被 称 为 连续 的 一 致 系统 。 

2) 无 解 并 且 系统 被 称 为 非 连续 性 系统 。 

3) 存在 着 多 个 解 ， 系 统 被 称 为 未 定 系统 。 

友 例 9-4 病态 与 良 态 。 

如 果 在 矩阵 4 或 中 一 个 微小 变化 会 导致 所 求解 矢量 x 中 的 微小 变化 ， 那 么 系统 4Ax 一 被 
认为 是 良 态 。 如 果 在 和 矩阵 4 或 中 一 个 微小 变化 会 导致 所 求解 矢量 x 中 的 较 大 变化 ,那么 系统 
Ax 二 b 被 认为 是 病态 的 。 当 矩阵 A 中 的 行 或 列 彼此 接近 或 者 成 比例 时 ， 那 么 系统 是 病态 的 。 

考虑 下 面 的 方程 组 : 


























Ax =b 
| 3. | E) i a 
= (9-96) 
1 2 X2 1 
Xx] 一 1.0 
[E pa 
E) 1.0 
让 我 们 在 矢量 如 上 进行 一 个 小 的 改变 : 
2 3.991fzi 1. 98 
Ea 98) 
1 2 x2 1.01 
E) ie 9) 
= (9-99) 
£? 4.0 
MERNE EA PEAT AeA it) BL: 
k Ol 3. 国 | 
一 (9-100) 
0.99 2.01) lve 1 
XL 0. 1988 
eJ s on 
X2 0. 3993 
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该 方程 组 的 解 是 为 


此 时 的 解 变 为 


求解 方程 ， 得 


ws: 运动 学 、 动 力学 与 控制 技术 





因此 , 方程 组 [ 式 (9-96) | 是 病态 的 ， 并 且 对 4 或 者 中 的 扰动 比较 敏感 。 然 而 ， 该 


方程 组 满足 : 
2 31fzi 1 
| | H | (9-102) 
1 2x2 1 


是 良 态 的 ， 因 为 4 或 者 bp 中 的 小 变化 不 能 剧烈 改变 其 解 。 
解 x 对 A 或 者 p 中 小 变化 的 灵敏 度 可 依据 和 矩阵 A 的 条 件数 通过 式 (9-103) 进行 测量 。 


























LAX l e nCA) laa ll (9-103) 
|| x || lA || 

其 中 ， 
con(A)= ||A —! || || A || (9-104) 














|A | EEA 的 范 数 。 如 果 con(AI=1, MAMA 被 称 为 完全 良 态 。 如 果 con (A) 
二 1， 那 么 矩阵 A 是 良 态 的 。 如 果 con(4) 之 1， 那么 矩阵 是 病态 的 。 事 实 上 ， 系 数 和 矩阵 4 W 
数 中 的 相对 变化 可 以 通过 条 件数 con (A) 被 放大 至 矢量 解 x 范 数 中 相对 变化 的 上 极限 。 

UE AA : 






































以 下 列 方程 组 开始 : 
Ax=b (9-105) 
并 且 将 矩阵 4 改变 为 4'。 这 时 解 x 将 变 为 x ， 此 时 有 
Ax'=b (9-106) 
因此 ， 有 
Ax=Ax'=(A+AA)(x+Ax) (9-107) 
这 里 
AA=A'—A (9-108) 
Ax=x'—x (9-109) 
扩展 式 (9-107), A 
Ax=Ax+AAx+ AA(x+Ax) (9-110) 
化 简 ， 得 
Ax=—A 1AA (x 人 Ax) (9-111) 
式 (9-11) 表明 : 
|| Ax || <A! |] |] AA |] x+ Ax || (9-112) 


式 (9-112) 两 边 乘 以 范 数 上 A 1， 则 有 式 (9-103) 成 立 。 

KBI 9-5 矩阵 的 范 数 。 

和 矩阵 的 范 数 是 一 个 比例 正 数 ， 可 定义 每 一 种 矩阵 包括 : TE, A PE. JEFE 
置 矩 阵 。 对 于 矩阵 范 数 有 几 种 定义 。 最 重要 的 定义 是 : 

















lalh = mex > la; | (9-113) 
|| A || 2 =à Àmax(AT—A) (9-114) 
Alle = 9-115 
| || = = max D) lay | ( ) 
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IA lle= >) >) la} | (9-116) 


i=] j=1 
FEF WA || A || tH OR Tk — BON PEA ||-。， 我 们 求 得 矩阵 A 每 
行 元 素 绝对 值 之 和 ， 并 且 选 得 最 大 值 之 和 。 无 穷 范 数 上 A || -是 对 于 式 (9-104) 求解 时 能 够 
被 接受 的 条 件数 con (A). 
作为 一 个 例子 ， 下 列 和 矩阵 的 范 数 为 








r 











1 3 =3 
A=|—1 —1 2 (9-117) 
2 4 二 
lA ||== max È Jay | 一 max((111 十 131 十 | 一 80D) Q —11+1—11+ 121, 
< k 
((2|+|4|+]—2|)}=max{7,4,8}=8 
(9-118) 
我 们 可 以 检查 两 矩阵 范 数 之 间 的 下 列 关系 : 
|| A+B || <||A || + IIB || (9-119) 
|| AB ||<||A || ||B || (9-120) 


9.2 EPER 


A RZ eR Fe et. SR TSE LU 因数 分 解 的 方法 可 以 简化 我 们 的 数值 计算 ， 
为 我 们 已 经 应 用 了 该 方法 来 求解 线性 代数 方程 组 。 
假设 矩阵 A 可 以 被 分 解 成 : 


A=LU (9-121) 
这 里 
air Giz Ain 
a a es g 
A= 21 22 2n (9-122) 
any an2 a nn 
1 0 0 
lz 1 eee 0 
L= (9-123) 
Lal Ln2 1 
ui) U12 Uln 
O uz U2n 
U= (9-124) 
0 0 aa Unn 





这 时 ,A 的 逆 和 矩阵 将 为 
A !=U ~L! (9-125) 
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证 明 : 
因为 L 和 U 是 三 角 和 矩 了 泗 ， 因 此 其 道 矩 阵 也 是 三 角 和 矩阵 。 和 矩阵 M 的 元 素 为 : 
1 0 eee 0 





























mizi 1 TEE, 
M=L != (9-126) 
M nl M n2 nr 1 
即 
i=j 
mi=—ly— >) lamy ji i=2,3,;",n—] (9-127) 
k=jtl 
矩阵 V 的 元 素 为 : 
Vil Vil2 V1l3 Vid 
O uz v23 va 
V=U l= (9-128) 
0 O U33 U34 
0 0 0 U44 
Bp 
a j=i i=n,n—l,=:,l 
vj = k (9-129) 
_ i 
= UikUkji j >i i=n—1l1,-,2 
Uij k=i+1 
例 9-6 通过 和 矩阵 求 逆 求 解 方程 组 。 
考虑 由 4 个 线性 代数 方程 构成 的 方程 组 。 
Ax=b (9-130) 
2 1 3 =3 2i 1 
1 0 =] =2 || 2 
= (9-131) 
0 2 2 1 X3 0 
3 1 0 2/Jli) (-2 
依照 LU 因数 分 解 算法 ,我 们 可 以 将 系数 矩阵 分 解 为 : 
A=LU (9-132) 
这 里 
1 0 0 0 2 1 3 —=3 
1/2 1 0 0 0 1/2 5/2 1/2 
L= U= (9-133) 
0 —4 1 0 0 0 一 8 =] 
3/2 1 1/4 1 0 0 0 13/4 
和 矩阵 区 和 FT 的 逆 和 矩阵 分 别 为 
1 0 0 0 1/2, 1 —1/8 15/26 
一 1/2 1 0 0 0 一 2 5/8 一 3/26 
L i= U l= (9-134) 
= 4 下 0 0 0 —1/8 —1/26 
—1/2 —2 —1/4 1 0 0 0 4/13 


因此 ， 方程 组 的 解 为 
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—5/2 

oe 3/2 
x=U iL ‘b= (9-135) 

142 

=2 

kB 9-7 LU 因数 分 解 方法 与 其 他 方法 的 比较 。 
每 种 非 奇 异 和 矩阵 4 可 以 被 分 解 成 上 三 角 和 矩阵 和 下 三 角 和 矩阵 ， 即 4=ELU。 这 时 ， 方 程 组 
Ax 一 bp 等 效 于 














LUx =b (9-136) 
方程 两 边 乘 以 L-! 后 ， 可 得 : 

Ux=L~'b (9-137) 
这 时 ， 问 题 被 分 解 为 如 下 两 个 新 的 方程 组 : 

Ly=b (9-138) 

Ux=y (9-139) 


将 矩阵 4 分 解 成 LU 所 要 求 的 计算 时 间 正 比 于 n3/3， 其 中 是 方程 的 数目 。 然 后 ， 求 
解 每 组 Ly 二 b 和 Ux 二 y 的 计算 时 间 正 比 于 n?/2。 因 此 ， 通 过 LU 因数 分 解法 求解 方程 组 的 总 
计算 时 间 正 比 于 22 十 23/3。 然 而 ， 高 斯 消 元 法 占用 的 计算 时 间 正 比 于 227/2 十 23/3， 其 中 正 
向 消 元 所 占用 的 时 间 正 比 于 23/3， 反 向 替代 所 占用 的 时 间 正 比 于 227/2。 

另 一 方面 ， 通 过 LU 因数 分 解 方法 求解 矩阵 的 道 矩 阵 所 要 求 的 总 的 计算 时 间 正 比 于 
4n3/3。 然 而 ， 高 斯 消 元 法 需要 的 是 m4/3 十 n3/2， HH: 

CN Cat n>2 (9-140) 

图 9-1 描述 了 函数 n4/3 十 n3/2 一 4n3/3 的 曲线 ， 并 且 展 现 了 高 斯 消 元 法 计算 次 数 相对 于 
LU 因数 分 解 方 法 而 言 以 多 快 的 速度 增加 。 作 为 一 个 例子 ， 对 于 6X6 矩阵 逆 ， 利用 高 斯 消 
元 法 我 们 需要 540 次 计算 ， 利 用 LU 因数 分 解 方法 则 需要 288 次 计算 。 
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图 9-1 LU 因数 分 解法 计算 次 数 与 高 斯 消 元 法 
计算 次 数 之 差 的 一 函数 曲线 


交 例 9-8 和 矩阵 的 分 块 求 逆 法 。 
假定 矩阵 了 可 以 被 分 块 为 : 








345 


ws: 运动 学 、 动 力学 与 控制 技术 








A B 
r-| | (9-141) 

CD 

je, EE T aE (9-142) 计算 : 

E F 
r= | (9-142) 

G H 

这 里 

E=(A—BD~'!)7! (9-143) 
H=(D—CA~'B)7! (9-144) 
F=—A~'BH (9-145) 
G=—D~'CE (9-146) 


有 了 时， 该 方法 是 一 个 快捷 求 逆 法 。 
太 例 9-9 ”解析 求 逆 法 。 
WE n Xn ERAS a ] 是 非 奇 异 的 ， 其 行列 式 不 等 于 零 ， 即 | A | 和 0， 我 们 可 以 通过 
伴随 矩阵 4* 除 以 矩阵 A 的 行列 式 计算 其 逆 和 矩阵 A-!。 























A! e (9-147) 
EREA BY FERRE ME (adjoint matrix) 或 者 转 置 伴随 矩阵 (adjugate matrix) 是 矩阵 A 

的 余子 式 矩 阵 的 转 置 和 矩阵。 
a 一 AcT (9-148) 

















HA 表示 的 和 矩阵 A WAR ok IME A 求 得 的 ， 通 过 其 余子 式 替 代 其 中 的 每 一 个 
元 素 。 与 元 素 4; 相关 联 的 余子 式 可 由 式 (9-149) 定义 。 
AS =(—DitiAy; (9-149) 
DH, Ay 是 矩阵 A Wij KEER, SEET 中 的 元 素 a, 相关 联 ， 则 必然 存在 着 一 个 次 要 
的 矩阵 A;; ， 它 是 等 于 子 和 矩阵 行列 式 的 值 ， 该 子 矩 阵 通 过 删除 矩阵 4 中 的 i 行 和 7 列 获得 。 
FEM A 的 行列 式 可 由 式 〈9-150) 计算 ， 


























|A|= aya (9-150) 
j=l 
因此 ， 如 果 有 
air, Qiz Qis 
人 二 |221 42 a»; (9-151) 


aaj. Azz. 33 


伴随 和 矩阵 As 的 元 素 为 


i aaz 493 
4 一 4 和 一 (一 1)? (9-152) 
Azo 433 
a21 493 
a31 433 
Qiy ai 
Aj, ~A33 = (—1)® (9-154) 
ao, 499 
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FARE A 的 行列 式 为 
|A| FA 1432433 一 Qi1423032 — A 19 9) Az FA 17 31 A 93 FA 91 A 13439 A134 9943, (9-155) 


作为 一 个 例子 ， 考 虑 一 个 3X3 矩阵 如 下 : 











3 4 8 
A=|7 2 5 (9-156) 
9 6 1 
IERE A 的 关联 伴随 矩阵 为 
一 28 38 24NT (一 28 44 4 
A*=AT=| 44 一 69 18 | =| 38 一 69 41 (9-157) 
4 41 —22 24 18 —22 
矩阵 A 的 行列 式 为 
|A | =260 (9-158) 
因此 ， 有 
—7/65 11/65 1/65 
A 一 六 19/130 一 69/260 41/260 (9-159) 
6/65 9/130  —11/130 


*% Bl 9-10 凯 莱 -哈密 尔 顿 CCayley-Hamilton) 矩阵 求 逆 。 
凯 莱 -哈密 尔 顿 定理 : 每 个 非 奇 异 矩 阵 都 满足 其 自身 的 特征 方程 。 一 个 nXn KEE A = 
Ca) 的 特征 方程 为: 
|A—AI|=P()=Atta,_,A™ 1 十 … 十 ai 十 ao 一 0 (9-160) 
因此 , mn Xn FEM A=Ca,,) 的 特征 方程 是 ”的 多 项 式 。 基 于 凯 莱 -哈密 尔 顿 定理 ， 我 们 
可 知 




















POSS" ta gk erred ay =O (9-161) 
st (9-161) 两 边 乘 以 4 1， 得 
1 
A =A +a, A"? Ha D (9-162) 
0 
因此 ， 如 果 
aii. Qiz 443 
A 一 |a as a3; (9-163) 


IAI= >) aA (9-164) 


ERE A 的 特征 方程 为 
PQ)=(|A-—AI| =A? + (—ay1 —a22 一 433)12 十 

















(G11 29 A 4949) TA 11433 — A 1343) F924 33 A 93. g9 AT 
A 114 934 39 FG 1291 G33 FA 134 9943) G41 A 994 33 A12431423 Ay) A 134 gy 
(9-165) 
作为 一 个 例子 ， 考 虑 如 下 一 个 3X3 FOF: 
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其 特征 方程 为 





1 2 3 
A=|4 6 7 
5 8 9 





A3 
因为 矩阵 A 满足 其 自身 特征 方程 ， 因 此 有 : 





A? 


得 : 


式 (9-167) 两 边 乘 以 A-!， 


16X4? 一 104 一 2 二 0 





16A? 一 10A4 


2 二 0 


A 1A3—16A 1A? 一 10A '!A 二 24 7 ! 


由 此 可 得 矩阵 4 的 逆 和 矩阵 为 








ji 
AT! z’ 16A — 10D 


9.3 非 线性 代数 方程 


逆 癌 运动 学 问题 最 终 以 一 组 非 线 怕 


com DS 


3\2 1 2 3 
中 6 7 
9 5 8 9 
3 —2 

一 3 va 

L =i 





f(q)=0 
或 者 
Ji (qı q2 ot 5Gn) =0 
f2(g1 q2 stts dn) 50 
fei 742 2 5Gn) =0 
这 里 的 函数 矢量 和 变量 矢量 分 别 为 

fi) qı 

fe) q2 
f= eee 1 

fn(q) dn 


为 了 求解 方程 组 [ 式 (9-171) ]， 我 们 从 一 个 猜测 的 解 矢量 gq‘ 开始， 并 利 月 


搜寻 一 个 较 好 的 解 





q“ F9 =q‘? —J7 (q‘? iq) 


AS (gq?) 是 在 矢 虹 5q Sb TET AR BE a AS HE TE EB 


348 





ffi 
| 


(9-166) 


(9-167) 


(9-168) 


(9-169) 


(9-170) 


耦合 代数 方程 表示 。 考 虑 一 组 非 线性 代数 方程 


(9-171) 


(9-172) 


(9-173) 


日 迭代 公式 





(9-174) 


(9-175) 
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利用 迭代 公式 式 (9-174)， 我 们 可 以 接近 所 期 望 
牛顿 -拉夫 森 迭 代 法 ， 它 是 用 于 求解 非 线性 代数 方程 台 





的 确切 解 。 基 于 猜测 解 的 迭代 法 被 称 为 
的 最 常用 方法 。 





一 组 非 线性 方程 通常 有 多 个 解 ， 月 














昌 于 求解 非 线 性 方程 的 牛顿 -拉夫 森 欠 代 法 ， 其 主要 的 


缺点 是 解 并 不 一 定 是 感 兴 趣 的 解 。 该 方法 所 提供 的 解 高 度 依赖 于 初始 估计 。 因 此 ， 正 确 的 佑 












































计 有 助 于 找到 合适 的 解 。 
证 明 : 
定义 增 量 5 为 
6 =q —g (9-176) 
Seq? 扩展 方程 组 : 
fq Sf aO HS (gq a’ (9-177) 
fiig tO ÆR (9-171) 的 确切 解 。 因 此 f(gq 1?) 一 0， 我 们 可 以 使 用 : 
JPE =— fq) (9-178) 
求 得 增 量 6 Oy 
6°0=—J-!i(g fq) (9-179) 
确定 式 (9-171) 的 解 为 
git =q —J (gq fq) (9-180) 
ATERA, BT WRG PF BR IE IE TE — PK ET OE. 
算法 9-3 MATS (gq) =0 的 牛顿 -拉夫 森 迭 代 法 ， 具 体 步骤 如 下 : 
1) 设置 初始 计数 值 i 二 0。 
2) 估计 一 个 估计 解 g4 一 9 。 
D 在 gq 中 处 计算 mees). 
J 
4) PAREREA JOSO =— fq? REE. 
5) WRIS? | <e, e 是 一 个 任意 公差 ， 则 g :就 是 方程 组 的 解 。 和 否则 计算 g TO = 
q” HeH, 


6) 更 新 设置 i 二 i 十 1， 返 回 至 第 3) 步 。 
例 9-11 2R 平 面 机 器 人 的 道 向 运动 学 问题 。 








2R 平面 机 械 手 的 端点 可 用 两 个 非 线性 代数 方程 描述 。 





f1(01,02)] __ 
四 CE a 十 12sin( 
假设 : 
站 一 六 一 1 
端点 位 于 
x) fl 
FFE 


寻找 能 够 提供 端点 期 望 位 置 的 关联 变量 为 


网 


简化 系统 方程 ， 求 得 方程 的 雅 可 比 和 矩阵 及 其 逆 和 矩阵 ; 











1, cos0; +l2cos(O; 十 02 ) 一 入 


(9-181) 


| 


0, +02)—Y 


(9-182) 


(9-183) 
(9-184) 
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动力 学 与 控制 技术 
af, 9fi 
110) ES 901 902 B —lısinfı —lesin(O, +62) —lesin(d, +02) 
a0; If, Af lı cos0; l2 cos(0ı +62) l2cos(01 +82) 
96, AO» 
(9-185) 
=j 一 /2cos(O1 +02) 一 /2Ssin(O1 +02) 
J100) =— , , (9-186) 
lLıl2sinf2 (2, cos@; +12cos(6; +02) 1, sinO; +l2sin(@; +62) 


过 设置 i 二 0 和 估计 一 个 估算 解 ， E 拉夫 森 迭 代 算 法 了 。 

















(0) — a1 | (9-187) 
q A» nA/3 
因此 ， 有 
(z 中- | (9-188) 
3 3 0 一 1/2 
/到 引 =| A | (9-189) 
3 3 J3 —1 
—1. 3094 
59 =F =| | (9-190) 
1. 4641 
更 好 的 解 为 
BND (x/3) (—1.3094) (—0. 2622 
= + = (9-191) 
A> x/3 1. 4641 2.5113 
在 下 面 的 授 代 中 ， 我 们 可 以 依次 求 得 : 
f 2 hea e T A 
= os = (9-192) 
A» 2. 5113 一 0. 80337 1. 7079 
(3) | .3317 i 0. 31414 J- Aal 
= Ae (9-193) 
A» 1. 7079 0. 068348) 1.63958 
f a) | .0176 多 er ]- F 0. | 
= + (9-194) 
A> 1. 63958 0. 06739 1.5722 
Í 6) | . 0013 [* sa & unl 
= + (9-195) 
A> 1. 5722 0. 139 1. 571 
01)6) (—0.3X1078 0.291078 —0.49x107!° 
= + = (9-196) 
A> 1.571 0.85X1075 1.571 
GD (—0.49X107!0) (—0.41X107!9} (—0. 49X 10710 
= + = (9-197) 
A» 1.571 —0. 2X107? 1.571 
这 个 结果 非常 接近 肘 向 下 时 的 确切 值 : 
0 0 
一 (9-198) 
0， x/2 


WB 9-12 PDA ARIE VA AT CTE PETE 


考虑 我 们 将 要 搜寻 下 面 方程 组 的 确切 解 。 
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y= Tlse J= lem (9-199) 
这 里 ，/ 是 未 知 数 的 数目 ; i 是 方程 数 。 
假设 对 于 一 个 给 定 的 y;， 则 有 一 个 可 用 的 近似 解 a; 。 确 切 解 g; 和 近似 解 a? 之 间 的 差 
异 为 











ô; =q; qj (9-200) 
这 里 ， 近 似 解 g? 的 方程 值 由 式 (9-201) 表示: 
Y;=fi(g)) (9-201) 
迭代 法 是 基于 6; 的 最 小 值 ， 求 得 下 列 方程 的 解 ， 使 其 解 尽 可 能 地 逼近 其 确切 解 。 
yi=fi(g} +6;) (9-202) 
该 方程 的 一 阶 泰勒 扩展 式 为 
yi=filqs + 5 3 二 03) (9-203) 
i=1 j 
我 们 可 以 定义 
Repa (9-204) 
和 残 差 量 
ri=yi-Y: (9-205) 
将 残 差 量 写成 
7 一 JC 十 OD(062) (9-206) 
KE, J ey Pe ZA AE AT OME. A 
1=(22) com 
Iq; 


求解 的 方法 取决 于 m 和 nn 的 相关 值 。 因 此 ， 必 须 考 虑 如 下 3 种 情况 : 
1) m 二 n。 假 设 雅 可 比 矩 阵 是 非 奇 异 的 ， 线 性 化 方程 
r=J6g (9-208) 


拥有 一 个 唯一 的 解 ， 这 时 可 以 利用 牛顿 -拉夫 
森 技 术 求 解 式 (9-199)。 分 步 求 解 过 程 如 图 
9-2 所 示 。 

求解 过 程 的 有 效 性 取决 于 迭代 被 执行 的 次 
数 ， 这 个 过 程 取决 于 ”的 初始 估计 和 雅 可 比 $a os 
FEE AY 7. EJ AE Be HEDT FE H AP a: AS HE — 


的 ， 因 此 它 可 以 产生 取决 于 初始 猜测 的 不 同 | ce oH e 
ae] 




















解 。 而 且 ， 如 果 解 的 初始 估计 在 算法 收敛 域外 












































衰减 ， 那 么 可 能 不 会 收敛 。 在 这 种 情况 下 ， 需 eae 
要 努力 获得 一 个 数值 解 。 
2) m 二 n。 这 是 超 定 情况 ,一般 情况 下 无 qD =qh 46H 
解 ， 因 为 未 知 数 的 数目 (例如 机 器 人 中 关节 的 | 
数目 并 不 能 充分 产生 一 个 解 ( 例 如 末端 执行 
器 的 任意 配置 )。 然 而 ,我 们 要 求 的 是 使 误差 图 9-2 用 于 求解 非 线性 代数 方 
(例如 位 置 误差 ) 最 小 化 的 解 。 和 
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考虑 问题 : 


min{ F = 之 | (9-209) 
或 者 ， 以 和 矩阵 的 形式 表示 为 
min| F=3) [yf (J WEy Fa) ]} (9-210) 
这 里 
W=diaglwi my) (9-211) 





这 是 一 组 权重 因子 ， 给 出 每 个 运动 方程 的 相对 重要 性 。 
当 式 (9-212) 成 立时 误差 将 有 最 小 值 。 








T n filq;) J=0 (9-212) 
Iq) = Gi 
或 者 ， 以 矩阵 的 形式 表示 为 
J™WLy— f(qoJ=0 (9-213) 
第 3 个 系数 的 泰勒 扩展 表明 对 估计 解 g* 的 线性 校正 为 
J'WLy—f(q* )] 一 J6=0 (9-214) 
校正 方程 为 
JT =WJ8=JTWr (9-215) 





这 里 , r 是 由 式 (9-205) 所 定义 的 残 差 矢 量 。 
权重 系数 W 是 一 个 正定 对 角 和 矩阵 ， 因 此 JTW 总 是 对 称 的 和 可 逆 的 。 它 给 出 了 雅 可 比 
和 矩阵 的 广义 逆 和 矩阵 。 

















J =(W'W)) yw (9-216) 
它 可 以 验证 特征 : 
J 1J=I (9-217) 
当 雅 可 比 和 矩阵 是 可 道 时 ， 式 (9-210 的 解 是 非 线性 系统 (9-199) 的 解 。 
3) m 之 n。 这 是 匈 余 情况 ， 通 常情 况 下 有 无 数 个 解 。 选 择 一 个 合适 的 解 并 使 其 在 某 种 程 
度 上 是 最 优 的 。 例 如 ， 让 我 们 寻求 式 (9-199) 的 一 个 解 ， 它 能 最 小 化 给 定 参 考 配置 g'” 的 
偏差 。 当 找到 约束 函数 的 最 小 值 时 ， 这 时 就 可 解决 求解 的 问题 了 。 

















min(F = [g4 Waa J] (9-218) 
sk (9-218) 服从 于 : 
y— f(gq)=0 (9-219) 
利用 拉 格 朗 日 乘 数 技术 ， 式 (9-218) MIÈ (9-219) 可 以 由 一 个 等 效 问题 代替 : 
a 
= (9-220) 
oq 
aG 
rm (9-221) 
上 式 的 泛 函 定义 如 下 : 
G=; lg- TWE q™ J+ATLy—f (q) ] (9-222) 
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RAPHE — PAA mtn 个 未 知 数 的 mm 十 n 个 方程 的 系统 。 





WLqa—q ]—JT=0 (9-223) 
y f(q@=0 (9-224) 
式 (9-223) 的 线性 化 可 提供 关于 位 移 校正 和 拉 格 朗 日 乘 数 变 分 的 系统 方程 。 
Wé—J*A'=0 (9-225) 
Wô =r (9-226) 


这 里 , a FEA 的 增 量 。 
将 从 式 (9-223) 中 的 第 1 个 方程 获得 的 解 6 代 入 到 第 2 个 方程 中 ， 则 有 











JW- JT ôA Sr (9-227) 
或 者 ， 基 于 位 移 校正 有 : 
ôq =W 1JT(JW 1JT) (9-228) 
和 矩阵 
J+ =W7IJT(JW JT)! (9-229) 
Xt Pay SHE EEJ, R (9-229) AAEE REE. ERY A EREE, 
JJ* =I (9-230) 





KE, EAS AE AT BAY. B 
本 了 = (9-231) 


友 9.4 EERE BE Be a DE HE OE 

















TEDL ait Ade OP. BRAN ths BE RTT EBL tie A RE PY EERE. I EYP 
AEB JI A — EIS AT AAS Sy OR AL it A AE TE BE 























= — oF, odn °K idn Ka Ok, gate 
J= Ca HE Gaye . R i (9-232) 
ko "ki kii ka-1 
oko Mod, ok Xd, 2 ok, Xd, ( ) 
mie 9-233 
ok, oR, sy ok 1 





这 里 ,ec; 被 称 为 雅 可 比 生成 矢量 。 


Oki ad, Oki aX adn 
ci 一 一 | ° (9-234) 
| oÈ, oÉ; 


"Ri 是 与 矩阵 "上 ;相关 联 的 矢量 。 这 种 方法 仅仅 基于 在 正 向 运动 学 中 所 求 得 的 连 杆 变换 矩 
阵 ， 并 且 不 涉及 微分 运算 。 


WEES k ; X 


























tk 1 °R; ook: PR (9-235) 
KERE h 是 在 全 局 坐标 系 中 关节 轴 方 向 上 的 单位 矢量 。 对 于 旋转 关节 ， 则 有 


Ok; dy 
ci 一 | (9-236) 
Ok, 
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对 于 楼 柱 关节 EHRT). MA 


证 明 : 
两 坐标 系 之 间 的 变换 


Cp=CTpPr 


它 是 基于 旋转 矩阵 R SMER Ed Z Sk A TE WAY RE: 


mn 
T= 
0 1 
引入 无 穷 小 的 变换 矩阵 
e is 
T= 
0 0 
将 会 导致 
orro =| A 
0 
这 里 
E ea) 
T! = 
0 1 
因此 ，65 是 无 穷 小 的 旋转 和 矩阵， 即 
0 一 680。 60， 
55 一 SRRT 一 | 00. 0 一 60， 
一 580， 00, 0 
6v 是 与 无 穷 小 位 移 相 关 的 矢量 ，。 
dv=6d—60d 


(9-237) 


(9-238) 


(9-239) 


(9-240) 


(9-241) 


(9-242) 


(9-243) 


(9-244) 


让 我 们 定义 一 个 6X1 坐标 矢量 ， 它 用 来 描述 末端 执行 器 的 旋转 坐标 和 平移 坐标 。 


它 的 变 分 为 

















雅 可 比 抢 阵 J 是 将 微分 关节 变量 变换 至 微分 末端 执行 需 运 动 的 矩阵 。 


| | 
ox 
00 





_IT@, _ 
éx= F dq=Joq 





FE IE A Bh P ot PAE A) ZEAE E Ti Ao AY RB 





oT. =T(q)=°T, q 
因此 ， 无 穷 小 变换 矩阵 为 


T= >) OTi) Tlg) 
i=l 


354 





'T2(q2)°T3 (ga Toto ye TT Can) 


OC !T;) 


saat] . > 
ogi T, (dn ) Ogi 


(9-245) 


(9-246) 


(9-247) 


(9-248) 


(9-249) 
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有 意思 的 是 ,依据 D-H 变换 矩阵 [ 式 (5-11) ], 2 Hee BE AY Oh ST A ME A 
形式 : 
CIT;) 
一 :1A， İİT; (9-250) 
Ogi 
cos0; — sinĝ; cosa; sinf; sina; a, cos; 
Si sinĝ; cos; cosa; —cos;sina; a,sind; IR, iid; 
eT; = = (9-251) 
0 sina ; cosa; di 0 
0 0 0 1 
我 们 可 以 求 得 旋转 关节 的 速度 系数 矩阵 A; 为 
有 一 外 0 0 
ilk. 1 0 0 0 
Hiec moe | (9-252) 
0 0 0 0 0 0 
0 0 0 
棱柱 关节 的 速度 系数 和 矩阵 A; 为 
0 0 0 0 
GO: ee 0 0 0 0 
一 14 人 ;1 一 4 人 pb 一 = (9-253) 
0 0 0 0 0 1 
0 0 0 0 
现在 我 们 可 以 表述 式 (9-249) 中 的 每 一 项 ， 其 形式 如 下 : 
OT1T;) 
oT" (qi)! Talg) go een 1T, (qn) =C;T (9-254) 
这 里 
PE ICTIT:) pi R 
Ci; 一 (0T117T， S! oe ee ee CTT K 'T;) ! 
= (COT TLT; 1) A; T; OT To Li T;) -I (9-255) 
= (0T To ?2T;, 1) T1 A; (CTi T2' 2T; 1) 一 1 
ST TST: Aga T ST OT 
ITIER, SERE C: 可 以 被 重新 调整 成 为 
of. og. og or 0g. _ 
cft e R E (9-256) 
0 0 
对 于 棱柱 关节 CRB). MWA 
ok, 0 
Ci; 一 (9-257) 
0 0 


Ci 有 6 个 独立 项 ， 这 6 个 独立 项 可 以 被 合成 为 一 个 6X1 矢量 。 该 矢量 可 形成 雅 可 比 矩 
阵 的 第 i 列 ， 它 被 称 为 生成 矢量 e;。 对 于 旋转 关节 ， 雅 可 比 生 成 矢量 为 


0 大 od, 
«| ie | (9-258) 
《Ri 一 1 
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对 于 棱柱 关节 ， 则 有 
«=f À (9-259) 
op- 
MERKE Ad; 说 明 坐 标 系 B; 的 原点 在 基体 坐标 系 Bu 中 的 位 置 。 因 此 ,dv 说 明 相 对 于 
坐标 系 B;-1 的 末端 执行 器 坐标 系 B， 的 原点 位 置 ， 并 且 表 述 在 基体 坐标 系 Bo 中 。 
因此 ， 描 述 机 器 人 了 瞬时 运动 的 雅 可 比 和 矩阵 为 















































PE odd, ‘hid, aus Eninde meas 
0ko 0h] PTA Okai 
例 9-13 ”关节 机 器 人 的 雅 可 比 和 矩阵 。 
在 例 6-8 中 ， 用 下 列 每 个 变换 矩阵 分 析 了 关节 机 器 人 的 正身 和 逆向 运动 学 。 
cosO1 0 sing | 0 cos0» 一 Sin0。 0 l2cosd2 
sind} 0 一 cosO 0 sind 2 COSO> 0 lesin@s 
oT 一 1T; = 
0 1 0 0 0 0 1 d2 
0 0 0 1 0 0 0 1 
cosO3 0 sin; 0 cos 4 0 一 Sin04 0 
sings 0 一 cos03 0 sinĝ4 0 cOSO4 0 
2 下 3 = ST, = (9-261) 
0 1 0 0 0 =i 0 L3 
0 0 0 1 0 0 0 1 
cosds 0 sind s 0 cos0e —sinds 0 0 
sings 0 一 cosg5 0| . sin06 cos0e 0 0 
1T; = °T; = 
0 il 0 0 0 0 1 0 
0 0 0 1 0 0 0 1 
关节 机 右 人 有 6 个 自由 度 ， 因 此 其 雅 可 比 矩 阵 是 一 个 6X6 FEE, BY 
J(q)=(e1(q) c2(q) … cel(gqg)) (9-262) 
该 矩阵 将 未 端 执行 器 的 平 动 速度 、 角 速度 与 关节 速度 4 相关 。 
v ‘ 
Jsa (9-263) 
@ 
对 于 旋转 关节 ， 第 i 列 矢 量 c;(g) 为 
ok, Xi ide 
ci Cg) 一 (9-264) 
oÈ; 
对 于 棱柱 关节 ， 第 ; 列 矢 量 c; (q) 为 
Of; 1 
“oo=| | (9-265) 





求解 第 1 列 。 雅 可 比 和 矩阵 第 1 列 有 最 简单 的 计算 ， 因 为 它 是 基于 zo 轴 的 作用 和 末端 执 
aed 的 位 置 。 基 体 坐 标 系 中 的 zo 轴 方 向 单位 矢量 为 
0 
ok 一 |0 (9-266) 
1 
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末端 执行 器 坐标 系 Bo 的 位 置 矢量 由 ”Te 直接 决定 的 "ds 确定 。 


tii ztl2 t13 tų 


ToT TT T T T| "o a (9-267) 
t31 zt32 t33 t34 
0 0 0 1 
ti4 
de =| to (9-268) 
t34 


这 里 
tı4 =de {sinfı sinf, sinf; 十 cosOllLcosOusin0icos(0 十 03 )+cosds sin(O2 十 03 ) ]} 十 
Z3cos0; sin(02 十 03) 十 do sin01 十 /2cosOlcosO2 
(9-269) 
t24 =d6{—cos6 sins sinds +sin@; | cosO4sinds cos(O2 +63 ) +cosd5sin(62 +63) J} 十 
lLasinĝı sin(@2 +03) —d»2cos@, +12 sin0; cosO， 





(9-270) 
134 一 deLcosousin05sin(0 +63) —cosOscos(O2 +63) |+d2sinOs 十 13cos(02 +63) 
(9-271) 
因此 ， 有 
0 一 1 O)fti4 “ta 
bo Xde =° ko ds=|1 0 Olftaa |=| tu (9-272) 
0 0 0J 34 0 
第 1 个 雅 可 比 生 成 矢量 为 
一 24 
ti4 
C1 : (9-273) 
0 
0 
1 
求解 第 2 列 。 基 体 坐 标 系 中 的 x1 轴 单 位 矢量 为 
0 cos#; 0 sin: 0 sind, 
0h, =9R,|0]=| sind; 0 —cosd; |}0|=|—cosd, (9-274) 
1 0 1 0 1 0 


cz 的 第 2 部 分 需要 "&1 和 位 置 拓 量 'd6 的 又 积 。 矢 量 'd6 是 末端 执行 器 在 坐标 系 Bi 中 的 位 
置 矢 量 ,， 然 而 它 必 须 在 基体 坐标 系 中 描述 ， 以 便 能 够 执行 又 积 。 一 个 比较 容易 的 办 法 就 是 求 
IF Rs Xide， 并 且 将 结果 转换 至 基体 坐标 系 中 。 











0 lz cosf2 十 13sin(0, 十 03 ) 
Xd =°Rı Cı XId6) 一 "R| oO xsin0 一 acos(02 +43) 
1 d2 








© 
> 
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coshı (一 /2Sin0y +13 cos(62 +43) ) 
=| sinfı (—l2sinO2 +/3cos(O2 +43 )) (9-275) 
lcosO +13sin(@2 +43 ) 








因此 ， 可 求 得 作为 6X1 的 矢量 c* 为 
cos01[ —lzsinGz 十 13cos(02 十 93)|] 





















































sind; |—l2sinO2 +/3cos(@2 +43) | 
L2cosf2 +13 sin(O2 +63) 
G= i (9-276) 
一 coSsO1 
0 
求解 第 3 列 。 基 体 坐 标 系 中 的 zs 轴 可 由 同样 的 方法 求 得 : 
0 sing, 
°k2=9R2?k2=9R1'R2| 0 |= | —cosh (9-277) 
1 0 
cs 的 第 2 部 分 由 ?ks X ?ds 求 得 ， 并 将 其 结果 转换 到 基体 坐标 系 之 中 。 
ZL3coSsO3 
2kz X? ds =| 13sin03 (9-278) 
0 
Ll3cos01sin(0, 十 03 ) 
OR: Ck X? d) =| 135sin0lsin(02 十 03 ) (9-279) 
一 /3cos(C0O2 十 03 ) 
因此 ，cs 为 
/3cosOl sin(02 十 03 ) 
lssin01sin(0, 十 03 ) 
一 /3cos(02 +03) 
C3 (9-280) 
sind, 
一 cosO1 
0 
求解 第 4 列 。 基 体 坐 标 系 中 的 z3 轴 单 位 矢量 为 
0 cos: (cos0» cosl; 十 Singy sin; ) 
0E =9R,!Ro?R3| 0| 王 | sind, (cosd2 sind3 十 singycosb3 ) (9-281) 
1 —cos(O2 +63) 
ca 的 第 2 部 分 由 ?Rs X 546 求 得 ， 并 将 其 结果 转换 到 基体 坐标 系 之 中 。 
0 0 0 
°R; Ck; Xde)=°Rsl [0] XO 11=|0 (9-282) 
| L3 0 


因此 ， ci 为 
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0 
0 
0 
voit cos@, (cos02 cos@3 + sin@2 sings ) Le 
sinfı (cos02 sinf 十 Sin0y cosO3 ) 
—cos(02 +03) 
求解 第 5 列 。 基 体 坐 标 系 中 的 z 轴 单 位 矢量 为 
0 sind; cos04 一 cosOl sinOs cos(02 +43 ) 
0%, =°R,| 0 |=| 一 cosgicos0 — sind) sinds4cos(42 +03) (9-284) 
1 — sin, sin (02 +43 ) 
cs 的 第 2 部 分 由 *ks Xd RA, FILE RPG SSE KD RAZ, 
0 0 0 
OR, Ck, X4d6)=9Ra| [0] X]0] |=]0 (9-285) 
1 0 0 




















因此 ，cs 为 
0 
0 
0 
am sinĝı cosy 一 cosOl sinf4cos(02 十 03 ) 
一 cosOl cosĝ4 — sinfı sin0 cos(0, +43) 
一 Sin04sin(0O2 十 03 ) 
求解 第 6 列 。 基 体 坐 标 系 中 的 zs 轴 单 位 矢量 为 
0 — cosl; cosOu sin(02 +83) — sinf, | sinfı sind, 十 cosOl cosh, cos(0 +63) | 
0&5 一 "R5|0| 王 | 一 Sin0lcos0Ousin(02 十 03) 一 Sin04[L 一 cosOl sin04 十 sin0lcosOucos(02 +63) | 
1 cosd, cos(@2 +63) —0. 5sin204 sin(@2 +43 ) 











(9-287) 
ce 的 第 2 部 分 由 ks X546 求 得 ， 并 将 其 结果 转换 到 基体 坐标 系 之 中 。 
0 (0 0 
"R5(CRA5X5d6) 一 "Riliolxlo|ll=|0 (9-288) 
1 0 0 








因此 ， c6 为 
0 
0 
0 
一 cosOl cosOu sin(0 +03) 一 sin04[sin0l sin04 cosO1cosO4cos(02 +43) | 
一 Sin0lcosOu sin(0 +43) —sin@.[. —cos6 sin04 十 sin0lcosOucos(0 +03) | 
cosQacos(02 十 03 ) 一 0. 5sin204sin(02 +03) 











(9-289) 
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9.5 本 童 小结 


机 器 人 运动 学 中 会 有 一 些 常用 的 数值 计算 。 对 线性 和 非 线 性 方程 组 求解 是 计算 逆 和 矩阵 和 
雅 可 比 和 矩阵 的 最 重要 的 应 用 。 对 于 线性 方程 组 ， 一 个 已 经 应 用 的 求解 方法 就 是 LU 因数 分 解 
法 ， 对 于 非 线 性 方程 组 求解 方法 是 牛顿 -拉夫 森 法 。 这 两 种 方法 已 内 艇 于 已 经 应 用 的 算法 之 
中 了 。 








习题 


9-1 标记 与 符号 。 

描述 下 列 这 些 符 号 的 含义 。 

DL 2)U 3)B 4)D 5) ua 6) di 

7) con (A) 8) |A|- 9) [Alhi 10) lAl IDe 12)¥ 


13) J 14)@ 15) @i 7A. IOT 1779 18)8 
9-2 LU 因子 分 解法 。 
对 于 下 列 的 和 矩阵， 利用 LU 因子 分 解法 求 得 相应 的 L MU., 














1) 
1 4 8 
A=|5 2 7 
9 6 3 
2) 
2 一 3 —8 
p= wo = SZ 
0 2 2 4 
3 1 5 —2 
3) 
一 人 =b 3 =3 %6 
1 3 —1 一 2 0 
C=} 1 2 2 4 一 2 
3 5 =z =] 
1 =p 2 1 1 


9-3 LUR ( 道 向 ) 法 。 

利用 LU 道 向 法 求 得 习题 9-2 中 矩阵 的 道 和 矩阵 。 

9-4 LU 计算 。 

利用 LU 道 向 法 计算 基于 习题 9-2 中 矩阵 的 下 列 矩 阵 的 逆 和 矩阵 。 
D=AB E=AB"! F=A`!B G=A™'B"! 

9-5 线性 方程 组 。 
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md 





利用 LU A Pe EO EB «2 FR SE AR RE oe = 4. eo = 1, v3 =2, 
321 +8x2+5xr3=6 
221 — Tx. +423=9 


zı +9x2—6x3=1 














9-6 一 组 6 个 方程 。 
利用 LU 因子 分 解法 求解 下 列 方程 组 ， 并 说 明 其 解 是 zx1 二 75、x2 二 52、x3 二 40 Mr = 
31、ZX5 王 22、ZXx6 二 10。 








lla1—5x2—-2x6 =500 
20271 +41x2—-152x3—-625 =0 
329 +7x3—4x4=0 














Z3 十 274 一 2Z5 一 0 
一 2z1 一 15z5 十 47z6 一 0 
3z 一 107z4 十 28z5 一 15z6 王 0 





9-7 ” 非 线 性 方程 组 。 
求解 下 列 方程 组 。 


X1X2—2x1—- x: =0 





xixe 221x222 2x? 上 4z1 一 2 
六 9-8 高 斯 消 元 法 。 
高 斯 消 元 法 不 适应 于 这 两 种 情况 : 除 以 零 和 四 含 五 人 误差 。 
用 LU 因子 分 解法 检查 除 以 零 的 可 能 性 。 
*9-9 ”作为 四 舍 五 人 误差 的 数字 减法 。 
四 舍 五 人 误差 是 数字 技术 中 的 通用 技术 ,然而 误差 随 着 减法 数 的 增加 而 增加 。 利 用 高 斯 
消 元 法 和 LU 因子 分 解法 求解 由 下 列 4 个 方程 构成 的 方程 组 ， 并 且 在 每 种 方法 中 数 一 数 减 法 








2 1 3 一 3Vfza 1 
1 0 =i -2||lz | 
i 
sh 0 Slee! 2 


9-10 来 自 变 换 和 矩阵 的 雅 可 比 矩 阵 。 

利用 来 自 连 杆 变换 矩阵 的 雅 可 比 矩 阵 技术 求 得 如 图 5-21 所 示 的 R || R || RF LAR 
雅 可 比 矩 阵 。 选 择机 械 手 的 一 组 样本 尺寸 和 运动 学 参数 求 得 雅 可 比 和 抢 阵 的 逆 和 矩阵 。 

友 9-11 BRIBE Bey AE FT EIEE., 

Al) FE A E FT ae he EE Ye AE PE AR ORR 7 El 5-26 所 示 的 球形 手腕 雅 可 比 矩 阵 ， 
假设 坐标 系 Bs 是 基体 坐标 系 。 

9-12 SCARA 机 械 手 的 雅 可 比 矩 阵 。 

利用 来 自 连 杆 变换 矩阵 的 雅 可 比 矩 阵 技术 求 得 如 图 5-23 所 示 的 RW R || R || P BLAS AE 
HY EL RES 

9-13 3 A HÆRER || R ALF AYE AT HERE. 

5-22 所 示 为 一 个 3 自由 度 RER || R OLR. AEK H EITA HE E AY METI e E E te 
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术 求 得 该 机 械 手 的 雅 可 比 矩 阵 。 
WIA 分 区 反 演 (逆向) 法 。 
对 于 习题 9-2 中 的 抢 阵 ， 利 用 分 区 逆向 法 计算 矩阵 的 道 和 矩阵 。 
友 9-15 SPATIE ESR IE RE 
利用 解析 法 和 LU 因子 分 解法 求 得 以 下 和 矩阵 或 者 一 个 任意 3X3 EY ME. EIT 
和 比较 算法 运行 数 。 























1 4 8 
A=|5 2 7 
9 6 3 








9-16 凯利 -哈密 尔 顿 矩阵 求 逆 。 
利用 凯利 -哈密 尔 顿 法 和 LU 因子 分 解法 求 得 下 列 矩 阵 或 者 一 个 任意 3X3 JE RE BY a AE 
阵 ， 并 统计 和 比较 算法 运行 数 。 
1 4 8 
5 2 "| 
9 6 3 














克 9-17 和 矩阵 范 数 。 
计算 习题 9-2 中 和 矩阵 的 下 列 范 数 。 








|A ||1= max >) |a; | 
1SjSn j=] : 
|A | =A ma (ATA) 
n 
IIA ||== max >) la; | 
1<i<n =] : 
n n 
|A ||r= DO 
$ 
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第 | 部 分 
机 器 人 动力 学 


动力 学 是 运动 的 科学 ， 它 描述 的 是 当 力 和 力矩 作用 于 刚体 时 ， 刚体 运 动 为 什么 出 现 以 及 
怎样 出 现 的 问题 。 运 动 被 认为 是 位 置 、 方 向 及 其 时 间 导 数 的 演变 。 在 机 右 人 学 中 ， 利 用 机 械 
手 运动 的 动力 学 方程 建立 用 于 控制 的 基础 方程 。 

在 机 器 人 系统 中 ， 连 杆 和 机 械 辟 作为 刚体 而 被 建 模 。 因 此 ， 刚 体 动力 学 特性 在 机 器 人 动 
力学 中 占有 中 心地 位 。 因 为 机 器 人 机 械 臂 彼此 相对 旋转 或 者 移动 ， 因 此 在 刚体 坐标 系 Bi 、 
Bemis Wace 中 或 者 在 全 局 坐标 系 G 中 ， 必 须 开 发 和 描述 运动 的 平 动 方程 和 旋转 方程 。 

在 机 器 人 动力 学 中 ， 有 两 个 基本 的 问题 。 

问题 1. 我 们 想 将 机 器 人 连 杆 以 一 个 确定 的 方式 移动 。 为 了 实现 确定 的 运动 ， 需 要 多 大 
的 力 和 力矩 ? 

问题 1 被 称 为 正 向 运动 学 ， 它 容易 求解 ， 因 为 它 需要 对 运动 方程 进行 微分 。 问 题 1 包括 
机 器 人 静 力 学 ， 因 为 指定 运动 可 以 是 其 他 的 机 器 人 。 在 这 种 条 件 下 ， 问 题 简 化 为 求 合 适 的 
力 。 然 而 ， 问 题 1 有 许多 重大 问题 ， 涉 及 机 器 人 运动 而 不 是 其 他 。 一 个 重要 的 例子 就 是 求解 
所 要 求 的 力 ， 该 力作 用 于 机 器 人 使 其 末端 执行 器 沿 着 给 定 的 路 径 移 动 ， 在 预定 的 时 间 内 从 开 
始 位 置 到 最 终 位 置 。 

问题 2. 完全 确定 作用 于 机 器 人 上 的 力 ， 此 时 机 器 人 怎样 移动 呢 ? 

问题 2 被 称 为 道 向 运动 学 ， 求 解 是 比较 困难 的 ， 因 为 它 需 要 运动 方程 的 积分 。 然 而 ， 类 
型 2 的 应 用 问题 的 变异 是 很 有 趣 的 。 问 题 2 实质 是 一 个 预测 因为 我 们 希望 求解 未 来 所 有 时 间 
的 机 器 人 运动 ， 当 每 根 连 杆 的 初始 状态 已 给 定时 。 

在 这 部 分 ， 将 开发 相应 技术 以 驱动 机 器 人 的 运动 方程 。 


























































































































加 速度 运动 学 


刚体 关于 整体 坐标 系 的 角 加 速度 是 刚体 的 瞬时 角速度 的 时 间 导 数 。 一 般 来 说 ， 它 是 一 个 
与 角速度 不 同方 向 的 矢量 。 本 童 我 们 回顾 和 发 展 机 器 人 的 加 速度 运动 学 。 





10.1 få WME JER F ff Dn iE E yE BF 


考虑 在 参考 坐标 系 G(OXYZ) 中 具有 固定 点 的 一 个 旋转 刚体 坐标 系 B(COzyz )， 如 图 
10-1 所 示 。 
对 于 固定 原点 刚体 坐标 系 中 的 一 点 的 速度 
矢量 来 说 ， 由 式 (7-2) 有 
Gr (4) =" v(t) 一 GOBGr (1 一 GOBXSr (t) 
(10-1) 
可 以 利用 式 (10-1) 求 得 刚体 点 的 加 速度 


























矢量 。 
村 Gd.. 
Gy O= (1) 一 GSBcr 一 GCCBXGr () 十 
G@BX (Gc@B XY (1)) (10-2) 图 10-1 参考 坐标 系 G(OXYZ) PRA M 
Cr (D= ati) Xr $axXnxor) SA E E OR 
(10-3) 
GQ@B 是 刚体 B 相对 于 基体 坐标 系 G 的 角 加 速度 矢量 。 
can = tean (10-4) 
Gop 一 GOB (10-5) 
利用 角 加 速度 矩阵 sas 和 旋转 加 速度 变换 矩阵 5Se 也 可 以 定义 加 速度 。 
Gr (1) 一 GSpcr (10-6) 
这 里 
GOB=6OB=( $u$i) (10-7) 
cSp=°RgoRp=capt+co, =( futute’ n?) (10-8) 


证 明 : 
求 导 式 (10-1) 可 得 
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Gr (t)=c@x Xr 二 ooBXSr 一 GCBXSr +6@8 X (G@p X “Fr ) (10-9) 
由 于 
pu 
© = $= | due (10-10) 
gu 
w1 burt pur 
a = fgûú+ $ù =| v: | 一 | du2t fuz (10-11) 
Ws giz + fus 
因此 , 我 们 可 以 从 式 〈10-3) 中 获得 并 将 其 写成 矩阵 形式 。 
Gr a)=( f utgi tg u?) r 一 GSBSr (10-12) 
并 且 可 求 得 旋转 加 速度 变换 矩阵 cSe 为 
GSB 一 GO 十 608 一 GOB 十 GO 和 一 fu tuth n? 
—w3— w; olo2 一 03 olaws 十 os 
=|wiwz Fös wlw; wzw] 
wiwa w: w2zw3 Føl wi ws 
— (ui—1)8? Wd i 6 uiusp2?tieptueg 
=|ururg?+usdtus $ (ui —1) 6" uzus$ġ?—ùip— u $ (10-13) 
ee ee uzusġ? ùig Hug 一 (zx —1)ġ? 
因此 ， 刚 体 点 的 位 置 、 速 度 和 加 速度 矢量 分 别 为 
Brp =azityjt+ezk (10-14) 
Gd 
Sup = rp = Te = cm X or (10-15) 
Gd? : 、 
Sap 王 “up 一 rp 一 de Brp =cap Xr +to@pXor =ca BX r 十 GOBXCGOBXcSr ) 
(10-16) 
刚体 坐标 系 中 的 角 加 速度 是 刚体 角速度 矢量 的 导数 。 为 了 说 明 这 个 ， 我 们 使 用 导数 传递 
公式 [ 式 (7-152)]， 有 
G Bd Bq ; 
Gar =- GO =F bon + on X bon = 7 ben = an (10-17) 
刚体 坐标 系 B 在 基体 坐标 系 G 中 的 角速度 总 可 以 被 表示 成 : 
GCB 一 GQBZ。 (10-18) 








KE, a. 是 平行 于 ca 8 的 单位 矢量 。 角 速度 矢量 和 角 加 速度 矢量 通常 情况 下 并 不 平行 ， 因 
此 ， 有 
tate (10-19) 
GAB ÉGOB (10-20) 
然而 ， 唯 一 的 特殊 情况 是 当 旋 转轴 固定 在 G 和 B 坐标 系 中 。 在 这 种 情况 下 ， 有 
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GAB =a =O= f ù (10-21) 
利用 罗 德 里 格 斯 旋转 公式 ， 也 可 以 说 明 。 
. Gq? Gd? 7 
G @ 3 = cr = lim; Ra, lim z(u *cosg Fusing +a? +1) 
go dt? ¿>o dt 
= $u$ un (10-22) 





例 10-1 单 摆 的 速度 和 加 速度 。 
附着 在 无 质量 杆 的 质点 悬挂 在 一 个 旋转 关节 上 ， 这 被 称 为 单 摆 。 图 10-2 所 示 为 一 个 单 
摆 。 局 部 坐标 系 B 附着 在 单 摆 上 ， 该 单 摆 在 全 局 坐标 系 中 旋转 。 质 点 的 位 置 矢 量 和 角速度 











矢量 为 
ar 一 天 (10-23) 
lsing 
Sr 一 SRBBr =| —lcosd (10-24) 
0 
Con = ph (10-25) 
co 一 RE @ B= $K (10-26) 
ala 
= 0 
cos[ ntg) sin{ 2 r+] Sa ca. 
GR = ie i 
a sinf atg) cos Zae) 0 cosg sing 0 (10-27) 
2 0 0 1 
0 0 1 











图 10-2 #48 
因此 ， 它 的 速度 为 
Bo =r +E @ pXBr =0+¢6k XU =4lj (10-28) 
Lé cosg 
Se =°Rr”v =| 74 sing (10-29) 
0 
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质点 的 加 速度 为 
Ba 一 Eo 十 BobXEm 一 87 十 REX17 =l j—lġ?i (10-30) 
l $ cosg —lġ? sing 
“a=°Rs"a=|7 ¢ sing +142cos¢ (10-31) 


0 
例 10-2 ”地 球 上 汽车 的 运动 。 
考虑 地 球 上 朝 北 并 且 位 于 纬度 30" 处 的 汽车 运动 ， 如 图 10-3 所 示 。 相 对 于 路 ， 汽 车 的 速 
度 是 Er 一 80km/h 王 22.22m/s， 其 加 速度 是 a =fr=0. 1m/s?。 地 球 的 半径 是 RR， 因 此 汽车 
的 运动 学 关系 如 下 : 








=> j=- (10-32) 

这 里 主要 涉及 3 个 坐标 系 。 刚 体 坐 标 系 B 附加 在 汽车 上 ， 全 局 坐标 系 G 建立 在 地 心 处 ， 

男 一 个 局 部 坐标 系 E 刚性 地 附着 在 地 球 上 ， 并 且 随 着 地 球 旋 转 ， 如 图 10-3 所 示 。 假 设 坐 标 
AE MG 在 某 一 瞬时 是 一 致 的 ， 那 么 汽车 坐标 系 了 的 角速度 为 








AZ 
oj 











a) b) 
图 10-3 地球 上 朝 北 并 位 于 纬度 30 Ak WARE oh 




















6EoOB 一 GOE 十 中 op= ®Ro(weK + 0 ?一 (wrcos0)2 十 (osing)? +4; 


一 (ogcosb)i 十 (osing)8 十 六 (10-33) 
因此 ， 汽 车 的 速度 和 加 速度 为 
By =r 十 EopX8r =0+Beaxp X RE =vi — (Roxcosd)j (10-34) 
wECOSO 
psj 
Ba 一 Bo 十 BopX8gum 一 di 十 (Rossin0)7 + X| —Rwr cosl 
weEsing o 
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kA . 
Rw cossin 


= 2Rw 8 sind 


vw E sind 


=ai+(Rogsind)j + 


1 
2 P 
——v*? —Rwp cos? l 


R 








a+Rwcos0sind 


1 
RY — Rwkcos?0 


(10-35) 


ai 项 是 相对 于 地 球 的 加 速度 ，(2RwEOsin9)7 是 科 里 奥 利 加 速度 ( 科 氏 加 速度 )， 一 v?/ 
RE 是 由 于 汽车 移动 而 产生 的 离心 加 速度 ， 一 (Rw$cos?0) 是 由 于 地 球 旋 转 而 产生 的 离心 加 





速度 。 
代入 数值 并 且 接 受 尺 =6.3677X106m， 则 有 











< A a 2 366. 25 
By =v i —(Rwpcosh)j 一 22. 221 —6. 3667x 108 7 Jeos 


24 X 3600365. 25 
= (22. 22i—402. 137) 
Ba =1.5662X10~-"7 +1. 6203 X 10-37 —2. 5473 X10-2k 















































(10-36) 
(10-37) 


例 10-3 角 加 速度 的 合成 。 
依据 式 (7-63)， 我 们 可 以 知道 彼此 相对 旋转 的 几 个 刚体 ， 它 们 的 角速度 是 相关 的 。 
00, =001 H10: H30; +++, a, (10-38) 
几 个 相对 旋转 刚体 的 角 加 速度 服从 相同 的 规则 ， 即 
0G, =001 HA 十 2063 t+, 1a, (10-39) 
KI 10-4 旋转 加 速度 变换 的 合成 。 
让 我 们 考虑 在 O 处 具有 固定 点 的 基体 坐标 系 Bo 中 一 对 相对 旋转 的 刚体 连 杆 。 连 杆 的 角 
速度 是 关联 的 ， 其 联系 如 下 : 
00: =001 +102 (10-40) 
00: =001 +10? (10-41) 
因此 ， 它 们 的 角 加 速度 为 
0G1 而 (10-42) 
"d 
0&2 = 502 =a + hae (10-43) 
002 =i +} A2 (10-44) 
类 似 地 有 
0S1 =001 二 001 (10-45) 
0S2=G2+ 0 3 (10-46) 
因此 ， 有 
oS 2 =o 2+ )@ 3 = 1 HG 9+ (o@1 tO 9)? =0@ 1 HA HO i HO 3 +200 110, 
一 0S1 十 1S2 十 200110， (10-47) 
并 且 ， 有 
°S, AS 41S (10-48) 





式 (10-47) 是 相对 的 旋转 加 速度 变换 方程 。 它 表达 了 多 杆 机 器 人 的 相对 加 速度 。 例 如 ， 
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考虑 一 个 具有 6 个 旋转 关节 的 6R 关节 式 机 器 人 。 在 基体 坐标 系 中 ， 末 端 执行 器 坐标 系 的 角 
加 速度 为 





o0; = 9 a, +a toa; Ha, Hia; 十 :Cl (10-49) 











S 
8 
wa 


oS =S, HIS H9S; HSS, HiS; HES, +2001 a@2.+$a3;+3e.1 
210: O0: +50, + $5 +i) +210: Co) (10-50) 
利用 旋转 矩阵， 我 们 可 以 将 刚体 点 PY Ae Je J EG 表达 式 和 刚体 加 速度 B 表达 式 彼 

此 相互 转换 。 


B Bp 6. —Ë G —B Gp B, —B Gp GpTtG B 
Gap = Reap—= RecSp rp =" RecSe Re re = Ro’ Rg Rg Rg rp 











=?RG° R g rp SRFS R B Tp =GSpg rp = (Gp + 60%) rp 
=a sp Xr teéo pX (Čo Xr) (10-51) 
Sap =°Reg Gap =°RpGS p” rp =° Rp ESB RE rp = Re RIS R BRB Tp 
=E R ERIO rp = oS porp = (cag HGO) r =c@ pX r tom pX (cag Xor) 
(10-52) 
依据 scSa MES, 的 定义 并 且 比 较 式 (10-51) 和 式 (10-52)， 我 们 可 以 实现 两 个 旋转 加 
速度 变换 矩阵 之 间 的 变换 。 








GSp 一 “RBESB RE (10-53) 
Sp—=“ RicSp" Rs (10-54) 
并 且 从 式 (10-55) 和 式 (10-56) 可 以 获得 式 (10-57) 
GRs=cSp° Rs (10-55) 
cR 一 GRaeSn (10-56) 
cSp Rg = RpGSs (10-57) 





如 果 它 们 被 表达 在 同一 坐标 系 中 ， 则 在 全 局 坐标 系 G 中 局 部 坐标 系 B 的 角 加 速度 是 在 
局 部 坐标 系 B 中 全 局 坐标 系 G 中 角 加 速度 的 负 值 ， 即 








Gop 一 一 ECc ca p =— fag (10-58) 
Cap =— paG ta BO Bac (10-59) 





icar X or 被 称 为 切 向 加 速度 ， 它 是 在 全 局 坐标 系 G 中 局 部 坐标 系 B 角 加 速度 的 一 个 
函数 。 全 局 加 速度 "a 中 的 项 cw spX (cwp Xor) 被 称 为 向 心 加 速度 ， 它 是 在 全 局 坐标 系 G 
中 局 部 坐标 系 B 角速度 的 一 个 函数 。 

HB 10-5 角 加 速度 和 欧 拉 角 。 

基于 欧 拉 角 的 角速度 Ewn 为 








wX 0 coso sind sing 9 lcosp 十 %sinlsinp 
C@z=|wy |=|0 sing —cosgsind ||0 |= ETET (10-60) 
wz 1 0 cos? 


y 0 十 cosg 
这 时 ， 角 加 速度 为 
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cose( 0 十 pysin0) 十 sinp(CWsing 十 0bcosg 一 0p) 
i gaS sing ( 0 pysing)+cosp (O09— sind +0 cosh) (10-61) 
p+ g cos0— 0 sind 
刚体 坐标 系 中 角 加 速度 矢量 为 
cocep —chsosp cWSsPD 十 cgcpSW sOsy 
bag = R cag =| 一 cpsV 一 cgcysp —spsptcOcocp shey Easg 
sso — cosh c0 





cosp( 6 +oysing) +sing(¢ sin + 6 gcosd— Å p) 
=| —sing( 6 + e¢sind) + cosg sind + 0 gcosd — Op) (10-62) 
¢ cosd—  —O gsind 
友 例 10-6 角 加 加 速度 。 
第 2 fa TINE EERE Xa =O 是 一 个 斜 对 称 和 矩阵 ， 它 与 角 加 加 速度 相关 X =a =e 。 


G 
GXB TUB EOE gf (ER a RE+°R, RI) =° R BÊRE +2° Rpp RI HORRI 











一 cUp 十 26SacOg 十 cOpcSE (10-63) 
位 于 Sr 处 刚体 点 P 的 全 局 加 加 速度 矢量 “7 为 
G 
Gj = T = 下 (esaecr j=% R a Rar 
=[gut2outgut3¢ $u? +2 uu Hp uutg n? ] r 一 GUBSr (10-64) 
CU =° R sRE 是 B 和 6G 之 间 的 旋转 加 加 速度 变换 和 矩阵。 因此 ， 角 加 加 速度 矩阵 为 
ju J12 J13 
=o = \9n jz jz (10-65) 





J31 J32 J33 


we 
HE 


ju=3uiuig? +3u?—1)$ $ 





ja =(2uzuı Fuzui)$? +3u2uid $ Cusp +H2u3 $ tus $ —us$3) 





j31 = (2uguituzui)¢?+3u3uid $ Cucd 2u2$ ur $ ur?) 




















fiz = (Quy ug tuyu2) $2? +3uiu2d p +Cu3d+2u3 $ +u3 $ —u3¢3) 
j22 =3uzu2ġ? +3(u3—lDd $ (10-66) 





732 = (2u3u,tu3u2)¢?+3u3u2d $ Cuig 2u $ ul $ ui?) 





jis=(2uiug3tuius)¢?+3uiusd $ Cu zg 2u2$ ur $ u2 $3) 




















j23—=(2usustusus) gurusg $ (uid 2u1$ ui $ uig*) 
j33=3u3u3s¢?+3CuZ3—l}d¢ 
K 10-7 基于 四 元 数 和 欧 拉 参数 的 角 加 速度 。 
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利用 基于 旋转 四 元 数 的 角速度 定义 


GOB 一 26e” (10-67) 
= (10-68) 
我 们 可 以 定义 角 加 速度 四 元 数 。 
pele e* T prze” (10-69) 
as F+2 ee" * (10-70) 


10.2 刚体 加 速度 


考虑 具有 附着 局 部 坐标 系 BCozyz) 的 刚体 ， 该 刚体 在 固定 的 全 局 坐标 系 G(OXYZ) 中 
自由 移动 。 刚 体 可 以 在 全 局 坐标 系 中 旋转 ， 同 时 刚体 坐标 系 BCozyz) 的 原点 相对 于 全 局 坐 
标 系 GCOXYZ) 原点 可 以 平 动 。 刚 体 点 P 在 局 部 坐标 系 和 全 局 坐标 系 中 的 坐标 如 图 10-4 所 
示 ， 可 由 式 (10-71) 关联 。 









@x[@ox(Srm-5m)] 





G G 
ax (Gr, — n) 











图 10-4 在 全 局 坐标 系 G(OXYZ) 中 自由 移动 且 建 有 局 部 坐标 系 B(ozyz) 的 刚体 




















Crp= Rp hp +° dp (10-71) 
这 里 , “ds 表明 移动 原点 o 相对 于 固定 原点 O 的 位 置 。 
BP 在 全 局 坐标 系 G(OXYZ) 中 的 加 速度 为 


Sap = ġp =] rp= cp X (rp —Sdy) + os X[ oX (Crp —"dg)] +° dp (10-72) 





证 明 : 
点 了 的 加 速度 是 对 式 (7-189) 或 者 式 (7-90) 求 导数 的 结 
G 
QP 一 dt Cúp 一 city X Erp tts X Srp + ds cite X re ces X Coma X Erp) + ds 
= oap X (orp —Sdy) +a, X [Say X (orp —Sdg) ] +E dp (10-73) 





Te @yX(G@pXirp) 被 称 为 向 心 加 速度 ， 并 且 与 角 加 速度 无 关 。 项 cms X Er pA 
切 向 加 速度 ， EHF Erp. 
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例 10-8 刚体 点 的 加 速度 。 
考虑 在 全 局 坐标 系 中 移动 和 旋转 的 刚体 。 利 用 位 置 矢量 的 二 次 导数 可 以 求 得 刚体 点 的 加 








速度 。 

crp 一 Rbarp 十 cd (10-74) 
cr 一 CR 十 Gd (10-75) 
Grp=°Rp rp +°d p =C RR R ac” r p—s d B) +E dg (10-76) 

对 式 (10-77) 给 出 的 角速度 矩阵 求 导 可 得 
cõp = Rp RL (10-77) 

G 
; “dd sk a sie pa Lee | a 
On = 7 cn =O Re Rt Re RE 一 有 Ra Rg + cOpcOn (10-78) 
因此 ， 有 

GR p°R}=cO_y—cOpo@s (10-79) 


所 以 ， 刚 体 点 的 加 速度 矢量 变 为 


Gr'p ="R Ri ra 一 ad) 十 Gd 一 (6@p—G@po@h) (rp 一 Gd ) 十 Gd (10-80) 


这 里 
0 — ws w2 
GOp 一 GOB 一 | ws 0 a) (10-81) 
w2 w] 0 
2 
w +w? T ww? —~W1W3 
GÕOpGOL =| 一 oloy witw, —wzws (10-82) 
2 2 
— @1W3 ~ W203 wi Fws 


例 10-9 2R 平 面 机 械 手 中 关节 2 的 加 速度 。 
2R 平面 机 械 手 如 图 5-9 所 示 。 肘 关节 绕 着 基体 关节 做 圆周 运动 。 已 知 : 


1 =b: $o (10-83) 
我 们 可 以 写 出 : 
001 =001= Éo (10-84) 
00, X’ r= G Ro Xr = 01Rz.0+00 r] (10-85) 
0 X Gø, X? r) =— 82 ri (10-86) 
计算 肘 关 节 的 加 速度 ， 有 
°F = FaR zotri — 02 r (10-87) 


例 10-10 在 移动 刚体 坐标 系 中 移动 点 的 加 速度 。 
假设 图 10-4 中 的 点 P 由 一 个 时 变局 部 位 置 矢量 ?rp (1) 表示 。 这 时 ， 通 过 应 用 导数 变 
换 公 式 [ 式 〈7-152)] 可 以 求 得 点 了 的 速度 和 加 速度 。 


Guop 一 Gd 十 BFrp 十 EapXBrp 一 Su 十 Bup 十 EapXBrp (10-88) 











Cap =° dat rele < ret ows XBrp 十 op X(Carp Ete XP?rp) 
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= d+ "ap +2608? op eopX rp tiop X (G@p Xrp) 


(10-89) 

假设 arp 和 天 0， 也 有 可 能 对 式 (7-186) 求 导 ， 则 可 得 点 P 的 加 速度 为 
Srp —="Ra Pa ds (10-90) 
Crp =°Rp"rp +o Rg rp + ody = coy Xo Re erp + Rp BF p + ody (10-91) 


E Fp =60n XoRp rp oop X Re rp + e@y X Rarp + Rv Fp +oRv Fp +° dp 
= 03 X Srp +o@p X (cp X rp) +2 oR X SF p+ orp +odp (10-92) 
右边 的 第 3  2c@n XSrp 被 称 为 科 里 奥 利 〈Coriolis) 加 速度 〈 科 氏 加 速度 ) 。 科 里 奥 
利加 速度 垂直 于 G6@B 和 ?rp。 





妈 10.3 加 速度 变换 抵 阵 


考虑 全 局 坐标 系 G 中 刚体 B 的 运动 ， 如 图 10-4 所 示 。 假 设 在 某 一 初始 时 刻 to 刚体 固定 
坐标 系 Booryz) 与 全 局 坐标 系 G(OXYZ) 重合 一 致 。 在 任何 上 尖 t 时 刻 ，B(Cozyz) AF 
合 于 全 局 坐标 系 GCOXYZ ) 。 因 此 ， 章 次 变换 矩阵 "Ta (1) 是 时 变 的 。 

应 用 齐 次 加 速度 变换 抢 阵 可 以 求 得 全 局 坐标 系 中 刚体 点 的 加 速度 。 
Sap() 一 SABSrp() (10-93) 





pap 











RE, SCA 是 加 速度 变换 矩阵 。 


RD Gee Ge TG 
GAR= Gap oo dz ( apn oo ) ds (10-94) 
0 0 
证 明 : 
基于 齐 次 坐标 变换 有 
Grp(t)=°Tg” rp =°TgeT3' rp (to) (10-95) 
、 GP GRT Gg _Gp GpTG ; 
Seti —"T T° rp = BaRa Ma Bo R da Grp (t) 
0 0 
Ka Gy EEE E : : 
| dp oes a (10-96) 
0 0 





d oes 

Sap (t= 75ST a rp =ST Bp (10-97) 
替代 Brp 可 得 

Can (t)=ST BET 519 rp(t) (10-98) 


取代 SsT p AIST 3), MUA 
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、 GR car [SRT —GRIGg,). GR GRT Gg@,—G R ORTS 
Cap (1)= Ra “dg | 7 2 Grp) = B B ds BOR p°dp 
o 0 人 0 1 


O — po Gg —_ a — 00l G ~ ~ i 
S OO “eae, ee”) 人 eerro (10-99) 
0 0 
这 里 


GP GRT 和 ~ 
Rsg“Rg =cSg = Gø 


B © 
* 10-11 平面 RR 机械 手 夹 持 器 运动 学 。 





T= ð- 00T (10-100) 






























































图 10-5 具有 关节 变量 0 M 0: 的 RR 平面 机 械 手 
10-5 所 示 为 一 个 具有 关节 变量 91 和 0, 的 RR 平面 机 械 手 。 连 杆 (1) 和 连 杆 (2) 
的 类 型 都 是 RR(C0")， 因 此 变换 和 矩阵 "Ti T: 和 ?Ts 分 别 为 
cosO1 一 Sin01 0 {icosO1 
sing cos 0 Lisin 
oT 一 (10-101) 
0 0 1 0 
0 0 0 1 
cos0» 一 Sin0。 0 />»cos0» 
sing COSO 0 lsin 
oe (10-102) 
0 0 1 0 
0 0 0 1 
cos(0ı +02) sin(0ı +62) 0 Zəcos(0ı +02) +L cosh, 
sin(@, +62) cos(@; +42) 0 desin€@; +62) +l, sind; 
T: = (10-103) 
0 0 1 0 
0 0 0 al 
点 M1 和 点 Ms 的 矢量 分 别 为 
ZicosO1 LocosO， 
à lı sin, i l2 sinĝ2 
rM 一 rM: = (10-104) 
0 0 
1 1 
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lə cos(01 


0 —üÜ = 
rm = Ti Irm = 


L2 sin(01 














02) +2, sind; 





ZL1cosOl 


(10-105) 


为 了 确定 点 Ms 的 速度 和 加 速度 ， 我 们 需要 计算 "T。 ， 可 对 "T， 直接 求 导 进行 计算 。 


=P oaths — O12c012 
= 612012 — 0128012 
t 
0 0 
0 0 
012 =01+0; 


012=01 二 0 


0 


0 


我 们 也 可 以 利用 链 规则 通过 "Ts =T T, HET. 





—/1,012s012—1101s01 


lo 61201» 


"T= TT Ta TT T+T: IT, 


这 里 
一 Sin01 一 cosO1 
or, >i; cosĝı — sind, 
0 0 
0 0 
—sing» 一 cos0;» 
COsO» — sind, 
T,=0, 
0 0 
0 0 





0 
0 
0 
0 
0 
0 
0 
0 


—/1sin01 
Lı cos), 
0 
0 
一 /2Sin02 
12cos02 
0 
0 


+1101c01 (10-106) 

0 

0 
(10-107) 
(10-108) 
(10-109) 
(10-110) 
(10-111) 





CAT, AT, 之 后 ， 我 们 可 以 利用 "TT7! MT) 求 得 速度 变换 矩阵 "V， 和 1IV，。 


cos 


opi 一 Sin01 





0 


sing 
cos0; 
0 
0 
sind» 
cosl? 
0 
0 


Or © O O = © O 


—l; 





oy =° TTT =ġ,?k 


ly, =T: T7 =ġ0;}k 
现在 ， 我 们 分 别 确定 点 M1 和 点 Ms 在 Bu 和 Bi 中 的 速度 。 





(10-112) 


(10-113) 


(10-114) 


(10-115) 
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ovm, =°V1? rm =b; (10-116) 


一 /Sin0， 
Z2cos02 
lou, =V! r m = 82 i (10-117) 
0 


为 了 确定 末梢 点 Ms 在 基体 坐标 系 中 的 速度 ,我 们 可 以 使 用 速度 矢量 加 法 。 


—(ġı+ġ2)lzsin(ĝı +82)—Åılısinðı 


owm, =° vm, H? om, =° vm, +°T1! vu, = (A, +6)l2cos(O 二 92) 十 O111cos01 





0 

0 
(10-118) 

我 们 也 可 以 利用 速度 变换 和 矩阵 "Vs Ri Um. 。 
Ms = V2 rm, (10-119) 
XE, EERME V 为 
0 一 0 一 0， 0 851, sind, 

oy, =° F, T7! = 0 十 0， 0 0 一 027icosgi (10-120) 

0 0 0 0 

0 0 0 0 


cos(@, +02) sin(@,; +62) 0 一 /2 一 /lcosO， 
—sin(@, +62) cos(CO1 十 02) 0 ZL1Sin02 
1 
0 





Onn—1 2 PE =1,— 
T 一 2Ti1To 一 1T710TT |= 


0 0 0 
0 0 1 
(10-121) 
而 且 ， 通 过 加 法 法 则 我 们 可 以 确定 速度 变换 和 矩阵 "Vs， 为 
0V 一 "0V1 十 ?Vs， (10-122) 
这 里 
0 一 0， 0 Aol, sind, 
0V, =° Ti T: T7 = 0， 0 0 — O211cos01 (10-123) 
0 0 0 0 
0 0 0 0 
因此 ,"ww 将 为 
ovm, 一 "V20rMs (10-124) 


为 了 确定 点 Ms 的 加 速度 ， 我 们 需要 通过 对 "Ts。 HARFA WOT 2. 
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- d- dd Ae, ae ; z eke ye 
"Tag T TO Sg OT T+T T= Ti Tt2 TT tT T 
(10-125) 
我 们 有 
— 6? c6,— 6 180; 62 s0,— 6 ch) 0 — 6 ,l180;—1, 6? Oy 
of = Sop, = — 8? s0 + ich — 62 c0 — 6130) 0 @ lichi — li 8280) 
t 
0 0 0 0 
0 0 0 0 
(10-126) 
— 83 cO2— 6 5802 62 s02— 6 z002 0 — 0 L280. —l2 02c 
i= Hr = — 82 s0 + 6 cb. 一 93c0 一 0 5802 0 0 ,12c02 —1202s0, 
t 
0 0 0 0 
0 0 0 0 
(10-127) 
因此 ， 有 
ru riz 0 ru 
m ds do. a 6 ss 2 0 re 
P= PTALA TT = O ie (10-128) 
0 0 O 0 
0 0 0 0 
rii =— 82, cosO12 — Å 12 8inO 12 
ra =— 025 sinĝ12 +8 12.cos012 
r12 = 67, sinf12 — Ô 12c0s012 
S . (10-129) 
r22 =— 04) cosO12 — O12 sind 12 
rii =— 02,l2cos012— 6 122 8inO12 — 621, cos0; — 6 ılı sinô: 


ra, = 02512. c08012 — Ô 12l28sin012 — Å? Lı sind, + O171cosO1 
EAT T, ACTS, BMA AAT Ty! ATS! OR MR EAE He E 
KEA A3 和 "4，。 





oa » 
0A = T OTT = 01 一 01 0 0 (10-130) 
0 0 0 0 
0 0 0 0 








ise 2 一 万 2 

14,=!T,1T;!= 0, 03 0 0 (10-131) 
0 0 0 0 
0 0 0 0 
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—03, —6 4 0 1, 03.cos0, +20; 621, cos0; + 6 211 sind, 





0A, =° T,’ T7! = 0 12 —03, 0 1102sin01 20102l1sin01— 0211cosO1 
0 0 0 0 
0 0 0 0 
(10-132) 
现在 ,我 们 可 以 分 别 确定 点 Mi 和 点 Mz 在 坐标 系 Bo 和 Bi 中 的 加 速度 。 
—110?cos01— ılı sin: 
am, =°A 2° rM, = 6 lı cosĝiı — l1 6? sind, (10-133) 
0 
0 
—12 032. cosb2— 6 212 sind. 
lay 一 14，1 rm: 一 6 512.cos02— 0312 sind, (10-134) 
0 
0 








— 1z 02, (cos012 +sinĝ12 )—0371cosO1 = 0,1, sind, 


— l2 ġ?, (cos012 —sinO 12) — 021; sind, + d ilı cos0 (10-135) 





Sam, =°A2° rm, 
0 
0 
Ke 10-12 ”加 加 速度 变换 矩阵 。 
遵从 相同 的 模式 ， 我 们 可 以 定义 加 加 速度 变换 
cjp() 一 “JaBcrP(i) (10-136) 
这 里 
、 tee pT —GpTc ste ts 二 、 
oa | Rs mee) GR BERT Cdp G R SREGdB E 
0 0 0 1 0 0 
GR ERT = Up = cp —2 (cp — 00) — Olc — 00 )T 
一 GXB 2( Gap 00')e'—o(.64,—aa')* (10-138) 





例 10-13 速度、 加 速度 、 加 加 速度 变换 矩阵 。 
速度 变换 矩阵 是 将 位 置 矢量 映射 成 速度 矢量 的 一 个 矩阵 。 假 设 p 和 g 表示 刚体 点 P 和 Q 
的 位 置 。 这 时 ， 有 








p) (10-139) 


cy On pp—i@pp VC TE Gi 
| -| p ~c@s P | t | =ov,[ ?| (10-140) 
0 0 0 1 
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KE, [V] 是 速度 变换 和 矩阵。 类 似 地 ， 我 们 也 可 以 获得 加 速度 方程 。 
Cq —°p = ¢@,(°q 一 5P ) 十 GOB(S9 一 5P )=o@p(°qg —®°p )+o@ sodp (gq —°p ) 


=c Op gq 一 "p )—c@prc@p( g 一 "pp ) 一 (GCC 一 GOBcOE)(CSg —"p) 





(10-141) 
式 (10-141) 可 被 转换 成 
“gq -ea [9 (10-142) 
0 
4 ` p 一 CCCB 一 cObpcoOT)cp 9 (10-143) 
0 0 


KE, A Fee Bll AS ize oH PRE ES TR FP 
再 求 导 后 便 可 求 得 加 加 速度 矩阵 或 者 第 2 加 速度 矩阵 。 


Gqg—°p=ods(°g —Sp) +263 Cq —°p )+c@p Cq —°p ) 











一 GZp(Sq Sp ) 一 (2c0p 十 cOpb)co(Ccqg —°p) 
一 [ea 一 (2608 十 cOps)608(Sg —°p) (10-144) 


式 (10-144) 可 被 转化 成 
G es" G 
| q J=% i (10-145) 
0 1 


这 里 ,J 是 第 2 加 速度 或 者 加 加 速度 变换 矩阵 。 














Ju Ji 
一 (10-146) 

0 0 
Ji 一 cc2p 一 (2005 十 Op)06 (10-147) 
Ju=°p —[6@p—(2c@gt+ 603) a5 jp (10-148) 


10.4 正 向 加 速度 运动 学 











正 向 加 速度 运动 学 问题 是 将 关节 加 速度 g 与 未 端 执行 器 加 速度 系 关联 起 来 的 方法 。 它 
们 的 关系 为 








x =Jq +Jq (10-149) 
XE, J Æ KER., q 是 关节 变量 矢量 ，g 是 关节 速度 矢量 ，g 是 关节 加 速度 矢量 。 
a=—(q1 q2 q3 do) (10-150) 
Qq=(41 G2 Gs Gud? (10-151) 
@=C¢qi Ge Ge = Gad? (10-152) 


RT X AUK 分 别 是 末端 执行 器 的 配置 速度 矢量 和 加 速度 矢量 。 


ov, od i a : 
*-| 上 0 =(X, Y., Zan Ox, OY, @z,)" (10-153) 
On 
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Lhd oa, od oe ee cin 
K=|, |=| "|=, Y, Z, òx, or öz)" (10-154) 
a, "a, 


因为 计算 雅 可 比 和 矩阵 的 时 间 导 数 J 通常 情况 下 并 不 是 简单 的 事情 ， 为 了 求 得 正 加 速度 ， 
最 好 使 用 式 〈8-126) 一 式 〈8-129) 。 





. 00; xX; Sd; OF R 1) 
sidl, (10-155) 
d:? k: tio: X; id; (对 于 了 关节 ) 
hog. x y cae 
| kit OOF REAP) (10-156) 


0 (对 于 了 关节 ) 
对 式 (10-155) 和 式 (10-156) 求 导 可 得 连 杆 G) 相对 于 前 一 连 杆 (i 一 1) 的 加 速度 。 


























o o0; X; Sd: tho: X Co: X; di) (对 于 R 关节 ) 
iid; = a £ a 
o0: X; "dd; 0@; X 0@; X; Gd:) +d: k; 2d: 001X?’ ki (对 于 了 关节) 
(10-157) 
G oop., D Om. OR. : y 
_6;= 0 ; ki-1 +0 i 9@ i—1 X kii OF R XT) (10-158) 
0 (对 于 了 关节) 
因此 ,未 端 执 行 器 坐标 系 的 加 速度 矢量 为 
od, = >) Od: (10-159) 
i=1 
0o, 一 >， fe: (10-160) 
i=l 
加 速度 关系 也 可 被 调整 成 递归 形式 。 
Aog. Og. Of. : y oe 
a tT ta et NTRA (10-161) 
0 (对 于 了 关节 ) 
例 10-14 2R 平面 机 械 手 的 正 向 加 速度 。 
求 得 2R 平面 机 械 手 的 正 速度 ， 即 
X =Jq 
X | Z1SO1 l»s(01 F02) l»s(01 H 6, 
一 (10-162) 
Y lichi +l2c(01 +42) loc(01 +02) ĝo 
AE AY FE FE RE BI FRON 
. du Jie 
J=| (10-163) 
Ja Jz 
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Jü 一 (一 /1cos01 — lz cos(0ı 二 9,))01 一 /2cos(O1 +02 )0， 

Jiı2=—lzcos(01 +62) 81 一 /cos(O +62) 62 

A : . (10-164) 

Ja =(—Llisinfı —lzsin(0ı +02))01ı —losin(O, +#02)02 

Jo2.=—lnsin(6, +62)64 一 /2Sin(CO 十 0 )0， 
因此 ， 机 械 手 的 正 加 速度 运动 可 以 被 调整 为 

X=Jq+Jq (10-165) 

然而 ， 对 于 2R 机 械 手 ， 更 容易 展示 的 加 速度 形式 为 


X pe | 6 oe ee) 6? 
y Ll1cos01 Z2cos(0; +02) 61+ Oe lisin: Zosin(@; +02) Ci +å)? 


(10-166) 








友 例 10-15 ”基于 位 置 拓 量 的 加 速度 。 
假设 机 械 手 末端 执行 器 的 位 置 矢量 r 作为 关节 坐标 g 的 函数 而 给 定 ， 即 
rıq) rı (qı »q2»q3) 
r=/r2@q) |=|r2(qi -q2°q3) (10-167) 
r3(q) r3(qi+q2+q3) 
末端 执行 器 的 速度 为 


or 








i =i Jog (10-168) 
这 里 
gri/9q1 9ri/Aq2 9ri/9g3 
Jp 一 光一 |3rs/ag， ars/agy Ara/aqs (10. 169) 


gr3/9qg1 97r3/ad 9r3/9g3 
矢量 r 的 二 阶 导数 为 





“OOF gang SOR N 
r ag! ela J (10-170) 








d (dr d 3. dJp. dJp. dJp. dJp. 
lr) =a 2 e ay aL dos (10-171) 


i=] 


10.5 反 向 加 速度 运动 学 





正 向 加 速度 运动 学 表明 ; 


ee a R 
i -( ‘asa +Jq (10-172) 


n 





RHEN EIERE J EIE EE ap REE, AA A E PE A ET AR FE Y 
速度 矢量 g 。 
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q =J (X —Jq) (10-173) 
iE BA: 
REMEN EERE J 是 非 奇 异 方 阵 ， 那 么 雅 可 比 和 矩阵 JJ A ee ee E E 
用 于 求解 一 组 线性 代数 方程 [ 式 〈10-172)]， 对 于 关节 加 速度 矢量 g 来 说 。 
然而 ， 随 着 机 器 人 自由 度 的 增加 ， 计 算 和 J -1! 变 得 更 加 麻烦 了 。 可 替代 的 技术 就 是 
将 方程 写成 一 种 新 形式 。 















































X —Jq=J4 (10-174) 
对 于 球形 手腕 机 器 人 来 说 ， 式 (10-174) 类 似 于 式 (8-222), 
X -jį É a (10-175) 
Sie pi 
或 者 
qı 
q2 
m A 0] 9s 
= 7 (10-176) 
n C D qa 
qs 
gs 
这 里 
m a ts 
E —Jq (10-177) 
n 
因此 ， 反 向 加 速度 运动 学 问题 可 被 求解 为 
6; 
0, ZA 'm (10-178) 
6; 
qı qı 
q: |=D |n —C| q> (10-179) 
qs q: 
和 矩阵 Jq =X —Jq 被 称 为 加 速度 偏差 矢量 
3 
odo= >) dw, X; d; (10-180) 
i=1 
© . ~ 
96= X0; Ria (10-181) 
i=l 
对 式 (10-180) 和 式 (10-181) RE, WE 
3 
oa eds = > LG, xX, $d: Ho: X Ga; Xd) ] (10-182) 


i=] 
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6 
ug 一 os 一 >) 6 PR 0@o;X0508 1) (10-183) 
i=1 























角 加 速度 矢量 uaw EX 的 后 半 部 分 。 这 时 ， 用 Ye ME JE 的 后 半 部 分 ， 可 得 偏差 矢量 
的 后 半 部 分 ， 即 
6 
5 00X G Ê (10-184) 


i=1 


取代 式 (10-182) 中 的 bw ;和 0@;， 有 


°0:= >) Oki (10-185) 
j=l 
ae 刀 oc 0 A OZ 
00 ; 一 5 (0; kj-1+)@; X 6; kj-1) (10-186) 
j=l 
od = 5 5 C; kj- to; XÅ; Ê; —1)X; ?di 十 5 5 CÊ; X(Co;X "di)] 
Zj] i=1j=1 
3 i A ` 3 i 。 ` 
= SS Oe bing Xr 55 (9@;_1X0;°k;_1)X, d:i + 
| | 
3 £ 。 、 
SYD 1 XGOw;X, di)] (10-187) 
i=1j=1 
则 可 求 得 偏差 矢量 的 前 半 部 分 为 








3 i 3 i 
DD Gop Xd rR; DX a + > D hX Gos X; di;)] (10-188) 
i=l j=1 i=l 7 一 1 


例 10-16 2R 平面 机 械 手 的 反 向 加 速度 。 
求解 2R 平面 机 械 手 的 正 向 速度 和 正 向 加 速度 分 别 为 


X =Jq 
X lisôı —l2s(01 +02) —lzs(01+0:)\ [0 
= (10-189) 
ý lichi +l2c(01 +02) l2c(01 +02) 0， 


X=Jq +Jq 


X Fo T 0 pa td ôi 
¥ li cosh, lzcos(O, +02) G1 二 6， lising! lz2sin(O1 +02) (0 十 02)2 















































(10-190) 
AE TE FE FE BE BY Se A Ee PEO 
. —1, 8,00; —l (6; +82 )c(O; 02) l2 (01 bz )c(0 +02) 
j= i i (10-191) 
—1101801 —l2 (01 +62 )s(01 +02) 12 (01 +02)s(01 +62) 
一 —15c(0; 十 0，) —l2s(0; +02) 
Tl (10-192) 
Li l2s02 1,00, +l2c(01 +82) 1,80, +l2s(6, +42) 


机 械 手 的 反 向 加 速度 为 
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q =J (X —Jq@) (10-193) 
sk (10-193) 可 被 调整 为 


0 加 1 locos(O; +02) Lzsin(0ı +0:)\( X 
ERT lL1l2 sin02 一 /1cosO1 一 /1Sin01 ï 


1 Lı l2 cosı L 0 
-R ' (10-194) 
1l2sing2( 一 /3 lil2cos0 J| (6, +å)? 


例 10-17 极 坐 标 平 面 机 械 手 的 反 向 加 速度 。 
10-6 所 示 为 一 个 具有 下 列 正 向 加 速度 运动 学 的 极 坐 标 平面 机 械 手 。 
X=Jg (10-195) 


X cosO 一 rsin0]| 二 
一 | (10-196) 
ý sind recos Jl ġ 


这 里 ，X 和 立 是 末梢 点 的 全 局 速度 分 量 ，J 是 机 械 手 的 位 移 雅 可 比 矩 阵 。 









































ee rsm] 
= (10-197) 
sind —rcos 
为 
图 10-6 极 坐 标 平面 机 械 手 
为 了 确定 末端 执行 器 的 加 速度 ， 我 们 对 式 (10-195) RKE., 
X =Jq +Jq (10-198) 
雅 可 比 的 时 间 导 数 为 
dfcosg —rsind 一 0sin0 一 rsin0 一 r0cosb 
| =| . (10-199) 
sinf reos0 0 cos0 r cosO 一 r0 sinf 





因此 ， 机 械 手 的 正 向 加 速度 为 


x cos? —rsinO){ Y 一 0sin0 —rsind—récosé \(r 
|. IH. | (10-200) 
Y sind cos 0 0 cos0 r cosO 一 r0 sind 0 
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为 了 确定 7 ， 我 们 求解 式 〈10-200) 可 得 ( Od", 


7 =I CX -Fq@) (10-201) 

















JETT FE JE PERI BE! 为 


_ 一 bsing —?sind—récosd = cos sind 
aa ; i 1 (10-202) 
0 cos0 Fr CoOSO—r0 sind =y sind 到 cos0 


因此 ， 机 械 手 的 反 向 加 速度 为 





gq =J X -J` Jq (10-203) 


5 COSO sind x 0 ee: dea 
= 1 1 — 3 g 
0 一 一 Sin0 —cosé || y l; 1; b 
r r r 六 


We10. 6 刚性 连 杆 的 递归 加 速度 











10-7 所 示 为 机 械 手 的 一 根 连 杆 (i) 并 说 明了 它 的 速度 和 加 速度 矢量 特征 。 基 于 刚性 














连 杆 的 速度 和 加 速度 运动 学 ,我 们 可 以 写 出 一 组 将 每 根 连 杆 的 运动 信息 关联 起 来 的 递归 方 
程 ， 





其 中 每 根 连 杆 所 在 的 坐标 系 附着 到 前 一 根 连 杆 上 。 这 时 ， 我 们 可 以 总 结 出 适合 于 具有 任 
意 根 连 杆 的 机 器 人 分 析 方 法 。 











d;i 


i 








图 10-7 连 杆 G) 的 矢量 运动 学 信息 


say 

















每 根 连 杆 的 平移 加 速度 由 "a; 表示 ， 在 质心 C; 处 测量 。 每 根 连 杆 的 角 加 速度 由 "w; 表示， 
通常 标示 在 质心 C; 处 虽然 对 于 一 根 刚性 连 杆 上 的 所 有 点 来 说 "w; 是 相同 的 。 我 们 通过 参考 
并 


连 杆 (i) 远 端 处 的 平移 加 速度 计算 连 杆 在 质心 C; 处 的 加 速度 。 因 此 ， 有 


Ca; =° di +a: X Cri — di) +0: XLo: X Cr:—’d:)] (10-205) 
st (10-205) 也 可 写成 为 
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矢量 


或 者 


ia:i=i d; +ia: X Cr;—'d;) tia; XE; X Cr; —'d;)] (10-206) 
JER G) 的 角 加 速度 为 





、 (10-207) 
o@i-1 (对 于 了 关节 ) 


式 (10-207) 在 局 部 坐标 系 中 可 以 写成 为 


a (e + d Ria “Fy @ 1 一 1 +6, ki (对 于 R 关节 ) 
owi = 








Pn Thai + 9; OR) HT: Coh-1 X 8,7 É) (对 于 及 关节 ) 
Ti" la; (对 于 了 关节) 
(10-208) 
证 明 : 
根据 式 (10-72) 中 的 刚体 加 速度 ， 点 C; 的 加 速度 为 
a= ooa X Crid Hoo X Do: X Cr TG, (10-209) 


我 们 将 质点 C; 加 速度 转换 到 刚体 坐标 系 B; 中 。 


ta; =9T7 a; =a; X Cr; —id; tie; Xlo: XCr:—id: J +d; (10-210) 

让 我 们 引入 矢量 "n; 和 "zm;， 以 便 相对 于 点 C 定义 of Mo; WME. 
n=? d;-,—° Fr; (10-211) 
om, =o d;—° r; (10-212) 
"72; 和 "ma; 可 简化 机 器 人 的 动力 学 方程 。 
对 于 相对 角 加 速度 的 加 法 ， 将 式 〈10-47) 或 式 (10-49) 作为 一 个 规则 ， 我 们 求 得 








oa; = a; t;a; + 0o X; Iw; (10-213) 
然而 ， 角 速度 ,w,; 和 角 加 速度 ,I@; 分 别 为 
,wj 二 0 有;-1 (对 于 有 关节) (10-214) 
9a,;=6,°Ri-1 (对 于 RR 关节 ) (10-215) 
;9w; 二 0 (对 于 了 关节 ) (10-216) 
; a; 二 0 (对 于 了 关节 ) (10-217) 
因此 ， 有 
a ;一 +8; Ri + es XG: k; (x R 3 F) 
oa; = Aig We a (10-218) 
0 @ 5] (对 于 了 关节 ) 


sk (10-218) 也 可 以 被 转换 至 任何 坐标 系 中 ， 包 括 B: 坐标 系 ， 以 求 得 式 〈10-208) 。 


ba; =T; |, a; =a; + 6 iki +i @ ;一 ! XO: Ri 
re (10-219) 
=iT C do; t 0:7 kii tH do; 1X 0;* kia) 
友 例 10-18 连 杆 G) 的 递归 角速度 方程 。 
EIF G) 的 全 局 递归 角速度 方程 为 
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.0 Bes 证 > 二 
Jo@ = o@;—1 +8; ki | (对 于 R 关节 ) (10-220) 
D; (对 于 了 关节 ) 
我 们 可 以 将 这 个 方程 转换 到 局 部 坐标 系 B; 中 ， 有 


i 0 个 一 1 0 中 一 1 h oF =i N) if 
o0; =T; o0; = T; O@;—1 FO: Ri-1) = 9 ; 10; Ri-1 





、 (10-221) 
=iT; er 1+0 ee Tee 


因此 ， 利 用 已 知 的 低 一 级 (i 一 1) 角速度 ,我 们 可 以 使 用 递归 方程 求 得 连 杆 (i) 
的 局 部 角速度 。 在 这 个 方程 中 ， 每 个 矢量 都 表示 在 其 各 自 的 坐标 系 中 。 


























iTia G ayy T k) OTF R 关节) 
(Oo; = | (10-222) 
Ti’ 00; (对 于 了 关节 ) 
*% 10-19 Æ G) 的 递归 平移 速度 。 
连 杆 (i) 的 全 局 递归 平移 速度 方程 为 
: °d; +o; X; ld; (对 于 R 关节 ) 
"d;=1 . . (10-223) 
"d; +0; X;_?d; 十 di" 有 ;1 (对 于 P 关 节 ) 
我 们 可 以 将 这 个 方程 转换 到 局 部 坐标 系 B; 中 ， 有 
idi =° T; od; =°T7 iat ese ki) 
=; ditia: Xaid td; ki- = Ti ditd he) +40; Xid, (10-224) 


因此 ， 利 用 已 知 的 低 一 级 连 杆 (i 一 1) 平移 速度 ， 我 们 可 以 使 用 递归 方程 求 得 连 杆 (i) 
的 局 部 平移 速度 。 在 这 个 方程 中 ， 每 个 矢量 都 表示 在 其 各 自 的 坐标 系 中 。 

















ae (TC ldi +d; 12:1) +4@: X; id; (对 于 及 关节 ) 
id; = (10-225) 
m oda t ed, OF PH) 
角速度 方程 是 相对 速度 方程 [ 式 (7-62) ] 和 基于 D-H 参数 的 刚性 连 杆 角速度 [ 式 
(8-2) ] 的 结果 。 平 移 速 度 方程 也 来 自 刚性 连 杆 速度 分 析 [ 式 (8-3)]。 
KG 10-20 递归 的 关节 平移 加 速度 。 
式 (10-205) 和 式 (10-207) 决定 了 基本 坐标 系 Bo 中 连 杆 G) 的 平移 加 速度 和 角 加 速 
度 。 利 用 连 杆 (i 一 1) 在 关节 i 一 1 处 的 平移 加 速度 ， 我 们 可 以 类 似 地 确定 连 杆 (i) 在 关节 
i 处 的 递归 平移 加 速度 。 

















es d;i toad; X; ld; toe; X (en; X,Yd;) OFT RAW 
d;= y 
0d. 1 +90); X; 2d too; X (oe; X; di) +4;°h:-1+2,0; Xd; R (对 于 P 关 节 ) 
(10-226) 
ix 
E Od; = q,—d,_ i (10-227) 


我 们 可 以 将 这 个 方程 转换 到 局 部 坐标 系 B; 中 ， 有 
id;=°T 19d, ='d,-, tid, X; id: +o: X Gm: X; id:) +d; h; i+; Xd; Ri 
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=T; Cd +d; kD) +25 o: Xd; T; kti o 
(wi;X, id:) +) @; X;-\d; (10-228) 
因此 ， 利 用 已 知 的 低 一 级 连 杆 (i 一 1) 平移 速度 ， 我 们 可 以 使 用 递归 方程 求 得 连 杆 (i) 
的 局 部 平移 加 速度 。 在 这 个 方程 中 ， 每 个 矢量 都 表示 在 其 各 自 的 坐标 系 中 。 











T; td tw X Ga: X; id 十 50; X, id; (对 于 R 关节 ) 
od jir, CRA td fb) +2) Xd Ti kh +i: x 
(a; X;-di) +) @; X;_\d; (对 于 了 关节) 
(10-229) 
式 (10-226) 是 求 导 式 (10-223) 的 结果 ， 式 (10-229) 是 将 式 (10-226) 转换 到 局 部 
坐标 系 B; 的 结果 。 
KW 10-21 旋转 关节 的 递归 加 速度 。 
如 果 一 个 机 器 人 的 所 有 关节 都 是 旋转 关节 ， 那么 关节 i 的 平移 加 速度 方程 [ 式 
(10-226)] 变 为 


















































od;=°d; toa; X Sd: +o; X Go; X;-4d;) (10-230) 
连 杆 G) 的 角 加 速度 变 为 
o0; =00; 1  ; Ri tHo: 1 XO ki (10-231) 
式 (10-220) 也 可 简化 为 下 列 的 连 杆 (i) 全 局 角速度 方程 。 
00; =00; 1+ 0 hi 1 (10-232) 
从 第 1 根 连 杆 开始 ， 我 们 求 得 如 下 的 前 4 根 连 杆 的 角速度 。 
æ =Å; $o (10-233) 
o0: = 0, +Å k =0, Ro +0, kı (10-234) 
03 =00: +03 Éo =O, ko +8 i +Å ho (10-235) 
o0, =; +Å, 3 =F Ro tO, ki H8 Ê HÒ, ks (10-236) 
连 杆 G) 的 角速度 为 
00;—=00; 1 二 0;"k; 1= 5 Ò, Èj (10-237) 
j=l 
前 4 个 角 加 速度 为 
oa, = 01°ko (10-238) 


o: = A, + Ri +0, Xb k= 01 kot 0 Ri H0 Ro XO k, (10-239) 
9&3 = G2 + Ü ks H00: X 0; É? 
= Phot hi +81 bo XG Rit O ka HO bo XO RI) XO k (10-240) 
0G, = 93+ 0 És How Xb, Ès 
= 8 kot d Rit 0 Rs tO Ro XO ki HA Ro Hi RX È 


388 


第 10 章 ”加 速度 运动 学 





0 d. 
1 


CÅ ho tH’ Ri H0 Rs) XO Rs (10-241) 
或 者 
oa, = 8 oX (H hi H0 ho +Å RHA hX CG hot Oy hs) tO Rk XO ks 
(10-242) 


因此 ， 我 们 可 求 得 如 下 的 连 杆 (i) 的 角 加 速度 为 
oQ; 一 ,1 十 0， oÉ i100 XÀ; Éi 


a d kj- + 5 (> 8, Rai XO 41°k;) (10-243) 


j=l j=l 





a T 2 (èa, > Ön kri) (10-244) 


j=l 


Im, AWR; XR; =0, SURURI BERR Ly 


=a; + 0 1 十 0@ XÀ; ka 5 0; 1+) B b, Êr 1X6; "hii ) 


j=l j=l k=1 


(10-245) 
利用 上 述 方程 ， 我 们 可 以 求 得 局 部 坐标 系 B; 的 平移 加 速度 。 


=d; +, a; X; 0d tom; X Gea; X;2d)= oa; X; 9d, + 2 wX Go; X; Sd) 


m 


-X C On Ên > (> OR X On kn-1)) X;-4d; + 


J= m=] m=1 


> È 9k m RE 8,° kn-1X; di )) (10-246) 


j=l m=1 m=l1 


WB 10-22 雅 可 比 速率 生成 矢量 。 
正 疝 加 速度 运动 学 需要 对 雅 可 比 矩 阵 求 导 。 








Cg od 
| |- "|=Jg +jg (10-247) 
@,) Vo, 
由 式 〈8-130) ， 我 们 知道 
oq, n oÈ- X; qd; . 
=>, 、 Å; (10-248) 
°o, =] oÈ; 


d, 
i 、 Gog. 0 
"d, “a, = kii Xd; T z a ki-1 X;-1dn) . 
-| “l-5 ED 8; (10-249) 
og a, i=] "kisi i=] diy; 
n a; ki-1) 


为 了 展开 这 个 方程 ， 让 我 们 从 和 ki FRB 
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—0k. a . KOR. 
Ri—1 @;i—ı Ri—1 
dt Hi 


利用 式 (10-237) 则 有 


1 一 ] 


d,a are R 
Lok, | 一 5 0; kj- X? Ê; 





dt = 
式 (10-249) 的 第 1 行为 
oe dÊ; . a 
a 2 0 Z 0 Of 0 
qo bint Xii da) 下 Xda t Ri- X 7d, 


使 用 式 (10-253) 和 式 (10-254) 求 得 第 1 项 为 








ldi =d; d; 
id, = > jaa; 
j=l 
dÉ; Sia x $ 
dt X,Yd,= (2 0;°Rj-1 X°Ri-1) X 2 ride 
j=l k=i 


=] n 
= 5 Chri X hX Vd; 
j=l k=i 
第 2 项 为 


n 


dt'~ 


(10-250) 


(10-251) 


(10-252) 


(10-253) 


(10-254) 


(10-255) 


、 d 、 d n d ` n 
"k; 1X 1da 一 ki X 5 ride =° ki- X 5 geid = ki X 5 OM, X idr 
k k=i 


i k=i 


n 





k=i k=i j=l 
n k : . : 
= 5 5 kj X OR j-1 xX, dd)0; 
Ret j=l 
因此 ， 有 
d i—l 、 、 n k 、 
A aE D X, Chja X? hia) Xd yp DD D eX 
j=l k=i j=l 


Chiat XxX, di)0; 
式 (10-249) 变 为 
py CÈ; <lh) xX; d;ĝ; 
oÈ; i=1 aij = 


D a a; oR ,1 XR 


j=l 





第 1 个 括号 是 雅 可 比 矩 阵 。 
a (RiXi td)  (°RoXGdn "RıXida «kiXs dd 
1 oo ok ie ok, 











J= 


oF 
i= ki -1 


第 2 个 括号 是 雅 可 比 和 矩阵 速率 。 





n k 
= D Ria XOX, Ad) = 2) ki X 3 0; °Rj-1Xp-Vde ) 


(10-256) 


(10-257) 


oy, n oR, 4 Kid, ` n n k _ _ . i 
| . 上 之 | 2 Ji D+ DD hi & Chj Xe ?dd | 8: 
il 


(10-258) 


| (10-259) 
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5 _ CÉ; XÉ) X; dj0; 


n n k 
j= 14 OD x CB Ka 8; (10-260) 
i=1 


== 
SI bE oh 

类 似 于 式 (8-119) 中 的 雅 可 比 生成 矢量 ec ， 我 们 可 以 定义 雅 可 比 速率 生 成 矢量 
a Chja Xk) Xj dj; 





Im 
加 
O. 




















n k 
Cigq)= + DD ka X OR X98; (10-261) 


k=ij=1 


D 7 Of Xp 5 0 


Jj 





以 便 逐 列 确定 J。 
JJ 一 (ce é éy) (10-262) 
KB 10-23 ”基体 坐标 系 中 的 递归 加 速度 。 
也 可 以 递归 性 地 计算 被 连接 连 杆 的 加 速度 。 对 于 连 杆 G 的 绝对 加 速度 ， 让 我 们 从 计 
算 "V; 开始 。 








0V; 一 "下 ;0T71 (10-263) 
这 时 ， 计 算 连 杆 (i 十 1) AYE RE RE 
Via = TT 一 (0 T: Tii HLT; T; HT; iT og TA 





SOT OTT a ae Te e eee T 
(10-264) 
这 两 个 方程 可 以 被 写成 递归 形式 。 
Wita =V; F2V OT Vi Tr H Ti Vi? T (10-265) 
对 于 一 个 旋转 关节 ， 则 有 





0 一 1 0 0 
. g F 。 1 0 0- 0 
IVi =d; ÅR =i ARS Âi (10-266) 
0 0 0 0 
0 0 0 0 
因此 ， 有 
© = 0: 0 1 0 0 0 
i ae 5 Tos 1 0 0 0 0 1 0 0 
iVit+1= 0,4, AR—074 ARAR liti — O54) (10-267) 
0 0 0 0 0 0 0 0 
0 0 O;-..0 0 0 0 0 


10.7 ”本章 小 结 


当 刚 体 坐 标 系 B 和 全 局 坐标 系 G 有 一 个 共同 的 原点 ， 坐 标 系 也 中 点 P 的 全 局 加 速度 为 
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F 二 一 0 vp=cQpX°r +co@p X (G@p Xr) 


Gq 
Gey 4,0" B 
然而 ， 当 刚体 坐标 系 B 相对 于 全 局 坐标 系 G 有 一 个 刚性 运动 时 ， 则 
ds 
dt 
这 里 , “ds 表示 刚体 坐标 系 B 的 原点 相对 于 全 局 坐标 系 G 原点 的 位 置 矢量 。 
两 根 连 杆 的 角 加 速度 依据 式 (10-271) 相关 联 。 


0G = a, +S a2 


Cap 














(10-268) 


(10-269) 


Cup =c@ pXSCrp— da)toon Xoos XC Crp— dn) ds (10-270) 


(10-271) 


利用 齐 次 加 速度 变换 矩阵 "4s ， 也 可 以 表述 在 全 局 坐标 系 G 中 具有 刚性 运动 的 刚体 B 


的 加 速度 关系 。 
cap() 一 "ADD9p(C) 

这 里 
cap — 00" Sdp — (cag —30T) dp 

0 0 

BL it AE [rl JE BE ie HAP KART HA (10-274) 定义 。 

X=Jq +Jq 

式 (10-274) 也 是 关节 坐标 加 速度 和 末端 执行 器 全 局 加 速度 之 间 的 关系 。 


工 


SA =S Tp Ts —_ 





q= Go Gy  G,) 
X=, Y, Zi oe òy, 0357 
这 样 的 关系 引入 了 雅 可 比 和 矩阵 的 时 间 导 数 J。 

















(10-272) 


(10-273) 


(10-274) 


(10-275) 
(10-276) 


已 知 机 器 人 的 正 向 加 速度 运动 学 [如 式 (10-274)]， 我 们 可 以 确定 机 器 人 的 反 向 加 速度 


运动 学 和 关节 加 速度 ， 对 于 一 组 给 定 的 末端 执行 器 加 速度 、 速 度 和 组 态 。 
=J X ji) 


习题 


10-1 标记 与 符号 。 
描述 下 列 这 些 符号 的 含义 。 


1) GCB 2) BCG 3) Gap 4) Bap 5) Bac 6) Cac 
7) 2@1 8) J ae 9) dan 10) 2a 11) ĉa 12) ta; 
13) Bey 140: 15) As 16)°V_ 17)J 18) X 


10-2 局 部 位 置 ， 全 局 加 速度 ，。 


(10-277) 





一 刚体 以 定常 角 加 速度 a 二 2rad/s? 绕 着 全 局 主轴 旋转 。 试 求 位 于 下 列 矢 量 处 
全 局 速度 和 全 局 加 速度 。 
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D 如 果 全 局 主轴 是 Z 轴 ， 角 速度 是 2rad/s， 旋 转角 是 x/ 3rad。 

2) 如 果 全 局 主轴 是 X 轴 ， 角 速度 是 1rad/s， 旋 转角 是 r/4rad。 

3) 如 果 全 局 主轴 是 Y 轴 ， 和 角速度 是 3rad/s， 旋 转角 是 r/6rad。 

10-3 ”全 局 位 置 ， 定常 角 加 速度 。 

一 刚体 以 定常 角 加 速度 a 二 0.2rad/s? RE Z 轴 旋 转 。 如 果 刚 体 坐 标 系 和 全 局 坐标 系 在 
:一 0s 时 刻 相 重合 ， 当 二 2rad/s 时 在 1 二 3s 之 后 ， 试 求 在 下 列 矢量 处 一 点 的 全 局 位 置 ， 
5 
30 
10 

















B= 








克 10-4 角速度 矩阵 和 角 加 速度 矩阵 。 

刚体 B 在 全 局 坐标 系 G 中 连续 旋转 。 变 换 矩 阵 可 以 通过 先 绕 着 Z 轴 旋 转 a 角 接 着 再 绕 
着 X 轴 旋 转 8 角 的 方式 进行 模拟 。 

1) 确定 作为 a 和 8 函数 的 旋转 轴 立 和 旋转 角 y 。 

2) 确定 如 果 a 和 8 随 着 恒 速 变化 ,那么 这 和 8y 也 变化 。 确 定 u 保持 不 变 的 条 件 和 y 保 
持 不 变 的 条 件 。 

3) 假设 让 是 常数 ，$ 一 5rad/s。 当 $= 二 30° 确 定 & FP. 

4) 4 a=30°, B=45°, a=5rad/s 和 B= 二 5rad/s Hf, M $. 

10-5” 绕 着 xz 轴 旋 转 。 

刚体 坐标 系 绕 着 x 轴 旋 转 ， 在 45" 处 角 加 速 和 角速度 分 别 为 一 5”/s? 和 35"/s， 确定 角 加 
速度 和 矩阵。 

10-6 角 加 速度 和 欧 拉 角 。 


如 果 欧 拉 角 及 其 导数 有 如 下 数据 ， 计 算 刚 体 坐 标 系 和 全 局 坐标 系 中 的 角速度 矢量 和 和 角 加 





























lt 


o=25rad 0 一 2. 5rad/s @ =25rad/s? 
0 一 一 0.25rad 0 一 一 4.5rad/s 0 =35rad/s? 


y=0. 5rad =3rad/s J =25rad/s? 
10-7 ”基于 欧 拉 角 的 角 加 速度 。 
利用 欧 拉 角 变 换 和 矩阵 完成 下 命题 。 
D 确定 笛 卡 儿 角 速度 ow 8 与 9、0 和 vy 之 间 的 线性 关系 。 
2) 确定 笛 卡 儿 角 加 速度 sw pS Oo. f 和 y 之 间 的 线性 关系 。 
k3) 确定 笛 卡 儿 角 加 加 速度 sj pS GC. OM 之 间 的 线性 关系 。 
10-8 合成 旋转 和 合成 角 加 速度 。 
刚体 坐标 系 绕 着 Z 轴 旋 转 30"， 接 着 绕 着 X 轴 旋 转 30"， 接 着 绕 着 Y 轴 旋 转 90"， 试 求 
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得 该 旋转 和 矩阵。 如果 刚体 分 别 以 角速度 4 一 20"/s RE Z 轴 旋 转 ， 以 角速度 8 一 一 40"/s 绕 着 
Y 轴 旋转 ， 以 角速度 7 二 55"/s RA X 轴 旋 转 ， 请 计算 刚体 角速度 。 如 果 刚 体 以 角 加 速度 


2 =2°/s 绕 着 Z 轴 旋 转 ， 以 角 加 速度 6 =4°/s? BBY 轴 旋 转 ， 以 角 加 速度 y =—6°/s? 绕 
AX 轴 旋 转 ， 请 计算 刚体 角 加 速度 。 
10-9 微分 和 坐标 系 。 
我 们 怎样 定义 这 些 表达 式 ? 
Gqcd.. sdcd cdBd Bdcd 
dt vad dż T dż au? dż oo 
“dda， ddo, -dda， Pd Pdi, 
dż dt dż dt dż dt dt dt 
10-10 BIH EMR fal. 
10-8 所 示 为 圆 环 路 径 上 的 一 个 滚筒 。 试 求 圆 简 圆 周 上 一 点 了 的 速度 和 加 速度 。 




















r 





=X 








图 10-8 PNR EWR fa 


10-11 RPR 机 械 手 。 
如 图 10-9 所 示 的 标注 坐标 系 ， 试 求 机 械 手 端点 处 一 点 P 的 速度 和 加 速度 。 








图 10-9 平面 RPR 机 械 手 


10-12 RRP 平面 匈 余 机 械 手 。 
10-10 所 示 为 一 个 具有 关节 变量 91 02 Mda 的 3 自由 度 平 面 机 械 手 。 
1) 求解 机 械 手 的 正 向 运动 学 ， 对 于 一 组 给 定 的 关节 变量 01, 02 和 ds 计算 末端 执行 器 
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图 10-10 ”RRP 平面 匈 余 机 械 手 





X.Y. o 的 位 置 和 方向 ， 其 中 X、Y 是 末端 执行 器 坐标 系 Bs 的 全 局 坐标 ，p 是 坐标 系 Bs 

的 角度 坐标 。 
2) 求解 机 械 手 的 逆向 运动 学 ， 对 于 一 组 XX、Y、o9 的 给 定 值 确定 01 02 和 ds。 
3) 确定 机 械 手 的 雅 可 比 和 矩阵 ， 证 明 下 列 方程 可 以 求解 正 向 速度 运 ans 











X Ay 
Y |=J| 62 
p ds 
4) 确定 J 1 ， 求 解 逆向 速度 运动 学 。 
5) 确定 了 ， 求 解 正 向 加 速度 运动 学 。 


6) 求解 逆向 加 速度 运动 学 。 
10-13 RPR 平面 见 余 机 械 手 。 
图 10-11 所 示 为 一 个 具有 关节 变量 91 、d。 M0; 的 3 自由 度 平面 机 械 手 。 























图 10-11 RPR 平面 元 余 机 械 手 
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D 求解 机 械 手 的 正 向 运动 学 ， 对 于 一 组 给 定 的 关 闻 变量 01 、0， 和 ds 计算 末端 执行 器 
X, Y., o 的 位 置 和 方向 ， 其 中 XX、Y 是 末端 执行 器 坐标 系 Bs 的 全 局 坐标 ，q 是 坐标 系 Bs 
的 角度 坐标 。 

2) 求解 机 械 手 的 逆向 运动 学 ， 对 于 一 组 X、Y、9 的 给 定 值 确定 0 、0> 和 4d 3。 

3) 确定 机 械 手 的 雅 可 比 矩 阵 ， 证 明 下 列 方程 可 以 求解 正 向 速度 运动 学 。 








X 0, 
Y |=J] 0， 
9 d; 
A) MEJ, REK RED. 
5) 确定 了 ， 求 解 正 向 加 速度 运动 学 。 


6) 求解 逆向 加 速度 运动 学 。 
W10-14 补偿 关节 式 机 械 手 。 
10-12 所 示 为 一 个 补偿 关节 式 机 械 手 。 














A 
3 
D 
% 
* 
dt 
出 
Ss 
= 
T 





1) 求解 机 械 手 的 正 向 

2) 求解 机 械 手 的 逆向 

3) 求解 机 械 手 的 正 向 

4) 求解 机 械 手 的 逆向 ? 

5) 求解 机 械 手 的 正 向 加 速度 运动 学 。 

6) 求解 机 械 手 的 逆向 加 速度 运动 学 。 

W10-15 修正 的 补偿 关节 式 机械 手 。 

10-13 所 示 为 一 个 具有 不 同 末 端 执 行 器 坐标 系 的 补偿 关节 式 机 械 手 。 确 定 该 机 器 人 的 
逆向 加 速度 运动 学 。 

%& 10-16 SCARA 机 器 人 道 向 加 速度 运动 学 。 


El 
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图 10-13 具有 不 同 末端 执行 器 坐 标 系 的 补偿 关节 式 机 械 手 

















10-14 所 示 为 一 个 SCARA 机 器 人 ,确定 机 器 人 的 逆向 加 速度 运动 学 。 











10-14 SCARA 机 器 人 


10-17 刚性 连 杆 加 速度 。 

10-15 给 出 了 刚性 连 杆 G) 的 坐标 系 及 其 运动 学 。 假 设 连 杆 的 角速度 和 近 端 、 
关节 的 速度 和 加 速度 已 知 ， 试 回答 以 下 问题 。 

D 根据 近 端 关节 i 的 速度 和 加 速度 ， 试 求 连 杆 在 其 质心 C: 处 的 速度 和 加 速度 。 

2) 根据 远 端 关 节 i 十 1 的 速度 和 加 速度 ， 试 求 连 杆 在 质心 C; 处 的 速度 和 加 速度 。 

D 根据 远 端 关 节 i 十 1 的 速度 和 加 速度 ， 确 定 近 端 关节 i 的 速度 和 加 速度 。 


= day 


4) 根据 近 端 关节 i 的 速度 和 加 速度 ， 确 定 远 端 关节 i 十 1 的 速度 和 加 速度 。 


aE 


Si 
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图 10-15 刚性 连 杆 (i) 的 坐标 系 及 其 运动 学 
友 10-18 滚动 盘 上 一 点 的 加 加 速度 。 
10-16 所 示 为 在 倾斜 板 面 上 一 个 滚动 的 圆 盘 。 试 求 点 也 在 局 部 坐标 系 和 全 局 坐标 系 
中 的 速度 、 加 速度 和 加 加 速度 。 





图 10-16 ”位 于 斜 板 上 的 滚动 盘 





398 





第 11 章 








运动 和 运动 变化 原因 之 间 的 关系 被 称 为 运动 方程 。 运 动 方程 的 导数 及 其 解 的 表达 式 被 称 


为 动力 学 。 本 童 我 们 将 回顾 运动 方程 的 构成 要 素 及 其 推导 方法 。 





11.1 WAI 


从 牛顿 学 说 的 观点 ， 作 用 到 刚体 上 的 力 可 分 为 内 力 和 外 力 。 内 力 就 是 刚体 内 部 粒子 之 间 
的 作用 力 ， 外 力 就 是 来 自 刚 体外 部 的 力 。 外 力 可 以 是 接触 力 〈 如 机 器 人 关节 处 的 驱动 力 )， 
也 可 以 是 刚体 力 ( 如 机 器 人 连 杆 上 的 重力 )。 外 力 和 力矩 被 称 为 负载 。 作 用 到 刚体 上 的 一 组 
力 和 力矩 被 称 为 力 系 统 。 合 力 F 就 是 作用 到 刚体 上 的 所 有 外 力 之 和 ， 合 力矩 就 是 外 力 绕 着 
一 原点 的 所 有 力矩 之 和 。 





F=) F; (11-1) 
M= >M; (11-2) 


考虑 作用 于 一 点 P WHF, AP AMER Ere 表示 。 力 绕 着 一 条 通过 原点 的 方向 线 的 
力矩 是 
M,=lu + (rp XF ) (11-3) 
这 里 ，u 是 表示 直线 1 方向 的 单位 矢量 。 力 矩 也 可 以 被 称 为 转 矩 。 
AF 作用 点 已 且 绕 着 位 于 矢量 ro 处 的 点 Q 的 力矩 是 
Mo =(rp—ro)XF (11-4) 
PAC. BS BY Hy 
M =r, XF (11-5) 
力 系统 作用 于 刚体 上 的 效果 等 效 于 力 系统 的 合力 和 合力 矩 的 效果 。 如 果 两 个 力 系统 的 合 
力 和 合力 矩 是 相等 的 ， 那 么 这 两 个 力 系统 是 等 效 的 。 力 系统 的 合力 有 可 能 为 零 。 在 这 种 条 件 
下 ， 力 系统 的 合力 矩 与 坐标 系 的 原点 无 关 。 这 样 的 合力 矩 被 称 为 转 矩 。 
相对 于 参考 点 P， 当 力学 系统 简化 为 一 合力 Fp MENEM, ， 我 们 可 以 将 参考 点 转换 到 
另 一 点 Q， 并 且 求 得 新 的 合力 和 合力 和 矩 。 





下 o =F, (11-6) 
Mo =M; + (rp —ro)X Fp =M, torp XFp (11-7) 

物体 的 动量 是 一 个 矢量 ， 它 等 于 物体 总 质量 乘 以 质心 的 平 动 速度 。 
p=mv (11-8) 


CD 
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动量 也 被 称 为 线 动量 或 者 平移 动量 。 考 虑 具有 动量 p 的 刚体 。 绕 着 通过 原点 的 方向 线 7 
AY oh E ELN 








L, =lu + (re Xp) (11-9) 
这 里 ，x 是 用 来 说 明 直 线 方向 的 单位 矢量 ，re 是 质心 C 的 位 置 矢量 , L=r, Xp 是 绕 着 原点 
的 动量 矩 。 动 量 矩 也 被 称 为 角 动 量 ， 以 区 分 于 线 动 量 。 

有 界 矢 量 是 在 空间 中 国定 于 一 点 的 矢量 。 滑 移 矢量 或 者 线 矢 量 就 是 在 作用 线 上 自由 移动 
的 矢量 。 自 由 矢量 只 要 保持 其 方向 ， 便 可 以 移动 到 任何 点 。 力 是 一 个 滑 移 矢量 ， 转 矩 是 一 个 
自由 矢量 。 然 而 ， 力 矩 取决 于 坐标 系 原 点 和 作用 线 之 间 的 距离 。 

牛顿 第 二 、 第 三 运动 定律 强调 力 系统 的 应 用 。 运 动 第 二 定律 也 被 称 为 牛顿 第 二 运动 定 
律 ， 它 强调 线 动量 的 全 局 变化 率 正 比 于 全 局 作用 力 。 






























































°F= “y= Sm w) (11-10) 
牛顿 第 三 运动 定律 ， 作 用 于 两 个 物体 的 作用 力 和 反作用 力 大 小 是 相等 的 ， 方 向 是 相 
反 的 。 
牛顿 第 二 定律 可 以 扩展 到 旋转 运动 。 因 此 牛顿 第 二 定律 也 可 说 成 角 动 量 的 全 局 变化 率 正 
比 于 所 施加 的 全 局 转 矩 。 
oy -dcr (11-11) 
dt 
UE AA: 
角 动 量 的 导数 为 
i 、 G x 
Sep = Gre Xp )= ep Hrex -or. xe SGreXSF=cM (11-12) 





处 于 移动 坐标 系 B 中 的 一 质点 PP， 其 质量 为 mw， 在 全 局 坐标 系 中 的 位 置 为 7 ， 速 度 
为 "op ， 质 点 P 的 动能 

















1 g — 
K =F mvp =m (Cdp Hup +O, X rp) (11-13) 
AACE 作用 于 沿 着 一 a 
做 的 功 为 
2 
W: = f °F .dr (11-14) 
il 
然而 
(oR dr= [fee edt =} | 2) dt=—m (w 3 vi) =K2—K 
r =n vaman a t g ENU? Vi )/—K2 1 
(11-15) 


式 (11-16) KH, JW: 等 于 在 终止 点 和 起 始点 之 间 的 动能 之 差 。 
\W2=K2—K, (11-16) 
例 11-1 质心 的 位 置 。 
在 一 坐标 系 中 ， 刚 体 的 质心 位 置 由 矢量 r 表 示 ， 通 常 在 刚体 坐标 系 中 测量 。 


"re=> | ram (11-17) 
B 
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1 
| xdm 
Le MB 
1 
yc |= Al ydm (11-18) 
mJB 
之 C il 
al zdm 
mJB 














对 处 在 具有 密度 o= 1 的 刚体 的 对 称 的 工 横 截 面 上 的 质 
心 进行 积分 ， 可 确定 横 截 面 中 的 质心 C， 如 网 11-1 所 示 。 























at | -1f b? Hab —a? g : DRN 
ZC 一 万 a7, „71A lab F24? (11-19) 图 11-1 对 称 工 截面 的 主 坐 标 系 
因为 对 称 ， 所 以 有 

_ b+ab—a? 


(11-20) 





vc TC Gab 2a? 

Bl 11-2 ”每 个 力 系统 相当 于 一 个 扭转 。 

ÆRE (Poinsot theorem): 每 个 力 系统 相当 于 一 个 单 力 ， 再 加 上 平行 于 该 力 的 力 
E. BERF 和 M 分 别 是 力学 系统 的 合力 和 合力 矩 。 我 们 将 力矩 分 解 成 平行 于 力 轴 的 平行 分 
EM, 和 垂直 力 轴 的 垂直 分 量 M，。 合 力 F 和 垂直 力矩 M_， 可 由 平行 于 F 的 单 力 F "所 取代 。 
此 ， 力 学 系统 可 简化 为 彼此 平行 的 一 个 力 F 和 力矩 M | 。 绕 着 力 轴 的 力 和 力矩 就 是 扭转 。 

潘 索 定理 类 似 于 沙 勒 定 理 (Chasles theorem), Wize BE. 每 个 刚体 的 运动 相当 于 平 动 
再 加 上 绕 着 平 动 轴 转 动 的 一 个 螺旋 运动 。 

Gl 11-3 在 移动 刚体 坐标 系 中 一 个 移动 点 的 运动 。 

如 图 7-5 所 示 ， 一 个 移动 点 P 的 速度 和 加 速度 在 例 10-10 中 已 被 求解 。 


Sy, =Ħ°d + Re Cup tho, Xr,) (11-21) 












































Sap = dst Re ap +22 wy X" vp thw, X rp) +°R Co X Cox’ rp) J 
(11-22) 
因此 ， 质 点 P 的 运动 方程 为 
SF 一 msa =m(od 十 SR Cap +28 os, X "vp Heop x Prp I+ 
mcR LE Oy X Bon Ps] (11-23) 
Bl 11-4 旋转 坐标 系 中 的 牛顿 方程 。 
假设 具有 固定 点 的 一 个 球形 刚体 (如 地 球 ) 以 定常 角速度 旋转 。 在 移动 坐标 系 移动 点 运 
动 方程 [R (11-23)] 中 ,设置 cd ,一 3%, 一 0， 可 以 求 得 刚体 上 移动 点 了 的 运动 方程 。 
SF=m ap Hmi @,X ConX rp)t2mi on Xr pm ap (11-24) 
上 式 表明 : 牛顿 运动 方程 下 二 ma 在 旋转 坐标 系 中 并 不 适用 。 
例 11-5 科 里 奥 利 (Coriolis) 力 。 
地 球 表面 上 移动 点 的 运动 方程 为 











BF =m™ap+mG@, X (CopX rp) +2mionX rp (11-25) 
将 式 (11-25) 重新 调整 有 
SF—miosX (ConX rp)—2meop X "vp =m ap (11-26) 
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式 (11-26) 是 对 于 人 处 于 旋转 坐标 系 中 观察 者 的 运动 方程 ， 它 就 是 观察 者 站 在 地 球 上 的 
情况 。 这 个 方程 的 左边 被 称 为 有 效力 。 
Fi ="°F— mo, X Co X rp) —2m bo, X vp (11-27) 
因为 实际 情况 看 起 来 好 像 是 物体 在 有 效力 的 影响 下 进行 的 移动 。 
第 2 项 是 离心 力 及 其 指向 的 负 值 。 地 球 上 这 个 力 的 最 大 值 位 于 赤道 


2 366. 25 
243600 365. 25 


它 大 约 是 重力 加 速度 的 0.3%。 如 果 我 们 加 上 重力 加 速度 的 变化 因为 从 地 球 北极 半径 
R=6356912m 至 地 球 赤道 半径 尺 二 6378388m 的 地 球 半 径 是 变化 的 ， 那 么 重力 加 速度 的 变化 
为 0.53%。 因 此 ， 一般 来 说 ,一 位 运动 员 ， 例如 在 北极 的 撑 笔 跳高 运动 员 ， 在 赤道 举行 的 
竞赛 中 可 以 获得 一 个 较 好 的 记录 。 

第 3 项 被 称 为 科 里 奥 利 效 应 ， 它 垂直 于 o 和 ?wp 。 对 于 在 地 球 北 半球 纬度 0 处 朝 着 赤道 
移动 的 一 个 质量 m 来 说 ， 我 们 应 该 提供 一 个 等 于 科 里 奥 利 效 应 的 侧 向 力 ， 以 迫使 质量 相对 
于 地 面 继续 沿 着 它 的 方向 运动 。 

F..=2m 6, XÂ? v, =1. 4584X107 pmcosd (11-29) 

科 里 奥 利 效应 是 铁路 、 公 路 和 江河 河床 西边 磨损 的 原因 。 缺 乏 科 里 奥 利 效应 是 转变 、 发 
射 体 的 方向 ， 掉 落 物 体 西 侧 的 原因 。 

例 11-6 定向 运动 中 的 功 、 力 和 动能 

假设 质量 m=2ke 具有 初始 动能 KK 二 12]。 质 量 在 常 值 力 F 二 Fi 二 4 的 作用 下 从 X00) =1 
移动 到 XX(z1) 二 22m。 在 运动 期 间 ， 力 所 做 的 功 为 




















二 6378. 388 10° x [ Leanne (11-28) 















































r 22 
W= F. dr= | 4dX =4X 21Nm=84]J (11-30) 
1 


r(0) 
在 终端 时 间 时 的 动能 
K(1)=W+K(0)=96] (11-31) 
式 (11-31) 说 明 质量 的 终点 速度 为 








2K ( 
v, £D =./96 m/s*9. 798m/s (11-32) 
Gl 11-7 HEH., 
当 应 用 力 是 时 变 的 ， 即 
F(t1)=mr (11-33) 
那么 运动 方程 有 一 个 通 解 ， 因 为 不 同 的 力 F(1:) 有 着 不 同 的 运动 特性 和 类 别 。 
;CD 一 Po 十 二 | F(t) dt (11-34) 
rr) H (toto) +— | | F(t) dede (11-35) 
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11.2 刚体 移动 动力 学 


11-2 所 示 为 全 局 坐标 系 G 中 一 个 移动 的 物体 B。 假 设 刚体 坐标 系 建立 在 物体 质心 C 
wb, RP 说 明了 物体 中 的 一 个 微 球 ， 它 有 一 个 非常 小 的 质量 dm 。 一 个 极 微小 的 力 dF 作用 
到 质点 dm 上 ， 并 且 产 生 了 一 个 全 局 速度 "wp。 




















I 





图 11-2 力 dF 作 




















具有 速度 ” wp 的 移动 质点 


根据 牛顿 运动 定律 ， 有 


d F=° ap dm (11-36) 
然而 ， 对 于 全 局 坐标 系 中 的 整个 物体 来 说 ， 其 运动 方程 为 
°F =m° a; (11-37) 
在 刚体 坐标 系 中 ， 式 11-37) 可 被 表述 为 
FF 一 me a, +me@, Xv, (11-38) 


在 这 些 方程 中 ,qs 是 物体 质心 C 在 全 局 坐标 系 中 的 加 速度 矢量 ，m 是 物体 的 总 质量 ， 
F 是 作用 到 物体 质心 C 处 的 外 力 的 合力 。 

证 明 : 

质心 处 的 刚体 坐标 系 被 称 为 中 心 坐标 系 。 如 果 坐 标 系 了 是 一 个 中 心 坐 标 系 ， 那 么 质心 C 
可 被 定义 ， 即 


























| "ra, dm =0 (11-39) 
B 
dm 的 全 局 位 置 矢 量 通 过 式 11-40) 与 它 的 局 部 位 置 矢 置 相 关联 。 
"Pye = dp Ry ra, (11-40) 


Tan dm = | “dy dm +°R, | Pran dm = | Cdsdm ="¢. | dm =m “dy (11-41) 
B B B B 


这 里 ,十 是 中 心 刚体 坐标 系 的 全 局 位 置 矢量 ， 因 此 ， 有 
| 
B 


对 式 (11-41) 两 边 求 取 时 间 导 数 ， 则 有 
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m| d; = | C Pan dm = | “vs, din (11-42) 
B B 
再 次 求 导 ， 则 有 
G 
mS vp=m" ap = | V,,, dm (11-43) 
B 


然而 , GR dF 二 ”Vp dm， 因 此 有 
may, = | dF (11-44) 


B 
对 式 (11-44) 右边 积分 ， 可 得 作用 于 刚体 中 极 微 小 质量 的 所 有 力 。 内 部 力 彼 此 抵消 ， 
因此 净值 是 所 有 外 力 的 矢量 和 F 为 


°F=m°a,=m"° vp (11-45) 
在 刚体 坐标 系 中 ， 有 


°F="R.°F=m PR ap =m Gap =m Pap HMO pX’ Up (11-46) 
11.3 刚体 旋转 动力 学 


刚体 的 旋转 运动 方程 就 是 欧 拉 方 程 。 














id ; . 
"M= L=" LicosXL= IG y+ GO, X CIGOp) (11-47) 
这 里 ， 工 是 角 动 量 ， 有 
"L="Ih@, (11-48) 
这 里 , 工 是 刚体 的 质量 矩 或 者 惯性 矩 。 
人 Tey Ts 
T= Ty le (11-49) 
Ter Tey Tex 
质量 矩 工 中 的 元 素 是 刚体 质量 分 布 的 唯一 函数 ， 可 由 式 11-50) 定义 。 
I= | (iO — Zink jn ) dm la] ~ L233 (11-50) 


B 
这 里 ，6;; BAGO 函数 。 
欧 拉 方 程 的 扩展 形式 为 


M =l wr Tiyw,y Lew: CL sip Tz wyw: — I y: (w? ~w) wa Ciel gy wyla) 








(11-51) 








My 一 Ta 十 To 十 Ta 


(Ts i We x = ks (wi —w? ) “Wy (wl yz wgl gy) 


(11-52) 











M.=I.,0, Toywy Irw — (Izr Iyy)wrwy — Ty lw, w) w yl a xl ye.) 
(11-53) 
在 被 称 为 主 坐 标 系 的 特殊 空间 笛 卡 儿 坐 标 系 中 , 式 11-51) ~ sh (11-53) 可 被 简化 为 
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Mı =I) 01 —CU2—13)w203 
My =I2H2—(13—11)w301 (11-54) 
M; =I1303—(11—I2)wiw2 
主 坐 标 系 由 数字 123 标识 ， 以 便 表 示 第 1 主轴 、 第 2 主轴 和 第 3 EM, EDA, BH 
IGA) ETE. 假设 刚体 和 主 坐 标 系 均 位 于 刚体 质心 C 处 。 
旋转 刚体 的 动能 3 








1 1 1 
K =F (wr HI yoy F102) 1 yore ~1y:@y02 — Tere 一 7O * L 一 了 oO To 
(11-55) 
在 主 坐 标 系 中 , w 11-55) 可 简化 为 
1 ; ; 
K= (Tiw? +I w +1303) (11-56) 


证 明 : 
假设 m; 是 刚体 B 中 第 i 个 微粒 的 质量 ,刚体 B 是 由 7 个 微粒 构成 , ri =P = Cry 
zi;)” 是 中 心 刚体 固定 坐标 系 Ozyz Pm; 的 第 卡 儿 位 置 矢量 , @ 一 C@g 一 Cw wy wy)T 
是 刚体 相对 于 地 面 且 表达 在 刚体 坐标 系 中 的 角速度 。 
质量 m; 的 角 动 量 为 
Li=ri Xmiri=milri X (@ Xr:)]=m:;:[ (r; ri)e@— (r; OOr; =mir? ø—m; (r; * Wr, 
(11-57) 

















因此 ， 刚 体 的 角 动 量 为 
L=) L= Dmirio— Dmi(r wr; (11-58) 


=] i= i=1 
Fest (11-58) 中 的 r; Mo., MA 
L Sw i 二 wy j +w: É) ` milz? +y? +z?)— >, mil(xiwr Fyiwy Fziwz) ° 
i=1 i=l 


(riit yij tzik) (11-59) 
因此 有 


工 一 > ， meet +y? re aig i 十 > m: læ? ty? +z? wy 7 十 ` mil? ty? tz? w, k— 
i=1 i=l i=l 
> 2i (Zioz 十 ylwy 十 ziwz)7i 1 一 > mi(xiwstyiwy Fziwz)yi j — (11-60) 


t=1 i=l 





` Mi Zioz 十 yiwy t+zjw.)z;k 


i=1 


或 者 








L= mila? ty +z? )wr —(Tiwr Hyiwy $zi02)2i it 























Sh mL? yi te? )wy—(riwetyiwy ziwz)yi it (11-61) 


i=l 
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> mille? +y? +z? ws — (tiw tyiwy Hziwz)zi jk 


i=1 


式 (11-60) 可 被 调整 为 


=> Emi (Cy? +27) Jor (> (mixiyidoy + pS (mixizi we )i— > Oniyixi os j +H 
i=l 


i= i=l i=l i=] 








en (2? +27) oy j 一 = (miyizi ws 了 一 D3 Gnizixi wx 十 > esau) + 
i=l i=l it i=l 























> Imi (a? +y?) Jor b (11-62) 
i=1 
角 动 量 也 可 以 被 写成 如 下 简约 形式 
Lz =] rwr Izywy Tisw: (11-63) 
Ly= ws toywy Ts (11-64) 
了 = 一 了 -cr Teywy Toews (11-65) 
By al S| AE A, tH Ee MET R 
L=I:+@ (11-66) 
Li Taz Tay Tg Wx 
Ly |= [Loe Tyy Tye || @y (11-67) 
Le Tey Tey I CU > 
这 里 
ILa = >) [mily? +z] (11-68) 
i=l 
Lyy = > [mi C? +27) ] (11-69) 
i=l 
I= >) [mi (x? +y?)] (11-70) 
i=l 
Tiy=1y=— >, Gniziy;) ClTY 
i=l 
E > niysz,) (11-72) 
i=l 
et p? Miz;x;) (11-73) 


i=] 
因为 微粒 是 一 个 连续 的 固体 ， 因 此 所 有 微粒 之 和 可 由 对 刚体 体积 的 积分 取代 ， 如 式 (11-50) 
所 述 。 
刚体 运动 的 欧 拉 方程 为 
G 


dp 
BM= yt (11-74) 


这 里 ,”M 是 作用 到 刚体 上 的 外 部 力矩 的 合力 矩 。 角 动量 并 是 在 刚体 坐标 系 中 所 定义 的 矢量 ， 
因此 它 在 全 局 坐标 系 中 的 时 间 导 数 为 


G 


bah +m, XL (11-75) 
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因此 ， 有 


Ma -i +o XL =lo+e@X (la) (11-76) 


SM= (1,02 HI yoy HI oi HU ye@2 tl yay Tw I + Tmt Ty tI) E+ 
wy (Iara tl ywy tlw, )i—we Iryor tlywy HI yw it 
wz (Triwr td Tiywy Tw )7 一 oz (Ticwr tT ywy Tw: y+ 


zzz 























wz (Iry@2 tlyywy tlw, )k—wy Twrtt Tywy tlw. dk (11-77) 
在 建立 在 质心 C 处 的 刚体 坐标 系 中 ， 刚体 运动 的 欧 拉 方程 的 最 一 般 形 式 为 
=] Towyt To — Uyy Twy we Iy (ww?) wr lwl ry wy) 
(11-78) 











M, =I. Tyyay Ty. (lee ~Iax We wy Irs (w? =w?) wy Gog ge — wT) 
(11-79) 





M.=Teus | Lawy | Tw (Izz Tyy wx wy Try lwt —w.) 一 wx- (wyl rz Twr] yz) 


x 





























(11-80) 
假设 我 们 可 以 将 刚体 坐标 系 绕 着 它 的 原点 旋转 ， 以 求 得 使 Ti =0. iA 的 方向 。 在 这 
样 一 个 被 称 为 主 坐 标 系 的 坐标 系 中 ， 欧 拉 方 程 可 简化 为 
Mi 一 Ti wi— (TC—13)w2w3 (11-81) 
Mz =I: ws— (3— Ti1)w3w1 (11-82) 
M3=13 ws— (Ti1—12)w1w? (11-83) 
利用 质量 微小 单元 dm 的 动能 对 整个 刚体 的 积分 ， 我们 可 以 求 得 刚体 的 动能 
K | jdm=> | (@ Xr) + (@Xr)dm 
ae nrg 
we ` we we ` 
= = | (y? +2?) dm 十 A (z? +x? ) dzz 十 J (z? +y? )dm 一 
Z Jg 2 Jg 2 Jg (11-84) 
ee] rydm—w,o. | _ zx dm 
B B B 
Eg +I yyw, 十 To? 区 —Irywrwy Í yzwywr I zrwzw zr 
en re ae 
ey a 。 
下 一 7 la=~@ L (11-85) 
当 刚 体 坐 标 系 是 主 坐 标 系 时 ， 动 能 将 简化 为 
K= diwi HI w +1303) (11-86) 
例 11-8 自由 旋转 刚体 的 等 角速度 转动 。 
刚体 牛顿 - 欧 拉 运动 方程 为 
GF=m" v (11-87) 
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"M =I6@, +60, XL 


考虑 这 样 的 情况 ， 作 用 于 刚体 上 的 合力 和 合力 矩 是 零 。 




















(11-88) 


(11-89) 


(11-90) 


然而 ， 欧 拉 方 程 简 化 为 


F=0 
M=0 
基于 牛顿 方程 ， 质 心 速度 在 全 局 坐标 系 中 将 是 一 个 常量 。 
. _T2—f1s 
Wl Ti W2W3 
+ _ 1:3—1i 
w2 I> W3W 1 
. hok 
w3 T; w1 w2 
式 (11-9) ~} (11-93) 表明 ， 角 速度 可 以 是 常量 ， 如 果 
I1=1;=1; 




















(11-91) 


(11-92) 


(11-93) 


(11-94) 


Be WAR PT EES CUNT MI 为 零 ， 则 起 初 的 第 3 角速度 〈 在 这 种 情况 下 为 ws) 为 








零 ， 或 者 起 初 的 角速度 矢量 平行 于 主轴 。 
例 11-9 两 杆 机 械 手 的 角 动 量 。 














一 个 两 杆 机 械 手 如 图 











AA MAMA G 是 重合 的 ， 因 此 坐标 系 A 和 坐标 系 G 之 间 的 旋转 矩阵 为 


cosp(1) 一 Sinp(CL) 0 
°Ra=|sing(t) cosg(t) 0 
0 0 1 








坐标 系 B 通过 欧 拉 角 (~=90°, 0=90°. Y=) 与 坐标 系 A KK. ALLA 











cacy 一 CTSTSW 一 CTSVW 一 CTSTCW STST 
^Ra 一 srep 十 crcrSW —sasp+enencey 一 cTST |= 
snsy sncy CT 


因此 ， 有 
—cospcosy— singsing 
“Rp =" Ry =| cosgsind — singcosy 


0 





— cosy 
sing 
0 


0 


sing 
cosy 
0 


cosgsinw — sing cosh 


cosycosw F sing sin 


坐标 系 G 中 坐标 系 A 的 角速度 和 坐标 系 A 中 坐标 系 B 的 角速度 分 别 为 


GoA 一 9 开 
AGOB 一 少 1 和 
机 械 臂 A 和 B 的 惯性 矩阵 的 矩 可 被 定义 为 
Inm 0 0 
AIA=| 0 Ix 0 
0 0 Im 
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0 
0 
—1 


0 
0 
=] 


11-3 所 示 。 连 杆 A 以 角速度 ? 绕 着 它 的 局 部 坐标 系 中 x 轴 旋 转 。 
连 杆 B 连接 到 连 杆 A， 并 且 相 对 于 连 杆 A 以 角速度 y 绕 着 zA 旋转 。 假 设 当 p=0 时 ， 坐 标 


(11-95) 


(11-96) 


(11-97) 


(11-98) 
(11-99) 


(11-100) 


Sis ano M 











le 0 0 
BIs=| 0 Im 0 (11-101) 
0 0 Te 
这 些 惯 性 和 矩 必须 被 转换 到 全 局 坐标 系 中 。 
SIA=" RepsIASRA (11-102) 
"T= ReSIp RE (11-103) 
机 械 手 总 的 角 动 量 为 
CL=°LA+" Ls (11-104) 
其 中 ， 
“La="Tacwa (11-105) 


GLs=" Tpowp=" lat ans eww) (11-106) 
kB 11-10 FER (Poinsot) 的 解释 。 
考虑 具有 主 坐 标 系 的 自由 旋转 刚体 。 已 知 M= 
0， 在 定常 角 动 量 和 动能 的 作用 下 产生 一 个 运动 。 
工 王 To 一 常数 (11-107) 








图 11-3 两 杆 机 械 手 








1 Te 
KK 二 一 @TI@ 一 常数 (11-108) 


2 
因为 角 动 量 的 模 是 常量 ， 因 此 有 
LSL LSL} +L) +L? 一 To 十 To 十 To (11-109) 
在 (wis w2, w) 主 坐 标 系 中 引入 了 一 个 椭 球 ， 将 此 椭 球 称 为 动量 椭 球 。 所 有 可 能 的 角 速 
度 矢量 末梢 必须 位 于 椭 球 面 上 。 动 能 也 可 在 相同 的 坐标 系 中 定义 一 个 能 量 椭 球 ， 以 致 角速度 
矢量 的 末梢 也 必须 位 于 它 的 表面 。 

















1 
= z Tie} HTao2 +1 sus) (11-110) 


换 句 话说 ， P A 的 角速度 需要 同时 满足 式 (11-109) 和 式 
(11-110)， 因 此 角速度 位 于 动量 椭 球 和 能 量 椭 球 的 交叉 部 分 。 
为 了 较 好 地 展示 ， 我 们 可 以 在 Les Lys Le) 坐标 系 中 定义 如 下 的 椭 球 方程 。 
L? +L? +L? =L? (11-111) 





L. L> , Li 
O1,K 21K  2I;K 
式 (11-111) 是 一 个 球 , 式 (11-112) 定 
义 了 具有 V21;K 半 轴 的 椭 球 。 为 了 具有 一 个 
有 意义 的 运动 ， 这 两 个 球体 必须 交叉 。 交 叉 
部 分 可 以 形成 一 个 轨迹 ， 如 图 11-4 ras. 
可 以 推断 ， 对 于 一 定 的 角 动 量 值 ， 必 然 
有 一 个 可 以 接受 的 动能 的 最 大 值 和 最 小 值 。 
假设 





=1 (11-112) 

















I, >1,>1T3 (11-113) 
6 的 动能 极限 值 为 图 11-4 ”动量 椭 球 和 能 量 椭 球 的 交叉 
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L? 
K min = 57 
L2 
K mex 37, 


相应 的 运动 是 分 别 绕 着 OT 轴 和 Ts 轴 的 旋转 。 
WK 11-11 欧 拉 运 动 方程 的 另 一 种 推导 。 
微小 力 dF 的 力矩 dM 为 
dM =° ry, XdF =° ran XÊ va dm 
dL=" ram XÀ Va dm 


根据 式 (11-11) 有 
G 


d 
dM~—— db 


G 


Pam rir e Fam xS Vdm dm ) 
对 整个 刚体 积分 ， 则 有 
G 


B B dt dt 
然而 ， 利 用 


G =G G B 
Fam 一 ds T R B PFdm 


这 里 ,di 是 刚体 坐标 系 原点 的 全 局 位 置 矢量 ， 可 以 将 积分 的 左边 部 分 简化 为 


6 ar ao ot 
| rin Xd F= | — (° Fam X Bin din) == | (© Tam X Van dm ) 
B 


(11-114) 


(11-115) 


(11-116) 


(11-117) 


(11-118) 


(11-119) 


(11-120) 


(11-121) 


| “rin Xd F= | Cd, +oRp rin) Xd F= | Sdn Xd F+ | Rg? ran Xd F 
B B B B 


="g.x°F+°M¢ 





XE, Me 是 绕 着 刚体 质心 CHAAR. 11-120) 的 右手 边 为 
G 


df je Sd ae , 
= | (Fam X Wam dm)=— | [Us + Rg Bran) X Van dm ] 
dt B dż B 


G G 
dt 
G 


、。 ; f = d 
=" dg X | © van dm +° dy X | € Dam dm +- Le 
B B dż 


我 们 用 Lc 表示 绕 着 刚体 质心 C 的 角 动 量 。 因 为 刚体 坐标 系 位 于 质心 处 ， 所 以 有 


z : f 
| "ram dm = m| dg =m Sre 
B 
@ a ee 
| Vim dm =m °dzp=m°vc 
B 
Ge _ G — G 
| Vam dm =m” dg =m”ac 
B 
因此 ， 有 
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d 本 a 、 dic l id 
ae | (Ud; x wa ) dm 十 一 一 | (Bram x Dam ) dm — —(G, x | CV am dm ) + —Lc 
B dt Jg dż B dż 


(11-122) 


(11-123) 


(11-124) 


(11-125) 


(11-126) 


Sis mio M 





Car a ; : ; Cas 
al, CÈ Fam XÊ vim dm)=" dy X| Ft Le (11-127) 
将 式 (11-122) 和 式 (11-127) 代入 式 (11-120)， 可 得 全 局 坐标 系 中 的 欧 拉 运动 方程 。 


该 方程 表明 外 力矩 绕 着 质心 C 的 合力 矩 等 于 绕 着 质心 C 角 动 量 的 全 局 时 间 导 数 。 








°M = doy (11-128) 
€ dt C 
利用 式 (11-128) 的 变换 ， 可 以 求 得 局 部 坐标 系 中 的 欧 拉 方程 。 
G 
Me 一 Ri M =°R} “dor = dege be Lo=" Le +® 0, ie 
(11-129) 
11.4 惯性 和 矩阵 的 质量 矩 

分 析 刚 体 运 动 中 出 现 了 属于 刚体 几何 的 两 种 类 型 的 积分 。 第 1 种 类 型 定义 质心 ， 当 考虑 











刚体 移动 时 便 会 出 现 。 第 2 种 类 型 是 惯性 和 矩 ， S 惯性 矩 也 被 
称 为 离心 力矩 ， 或 者 惯性 离心 力矩 。 每 个 刚体 都 有 一 个 3X3 的 惯性 矩阵 工 的 惯性 矩 ， 该 矩 
阵 定义 为 

















Izz Lay Las 
P=] Pye doy dk (11-130) 
Tex Tey Iz 
对 角 线 元 素 Ij Gj) 被 称 为 极 惯性 和 矩 。 
人 | Cy? 十 z2 )dm (11-131) 
B 
hans] (z? +x? )dm (11-132) 
B 
fh) (x? 十 y2 ) dm (11-133) 
B 
非 对 角 线 元 素 Iy GAZ) 被 称 为 惯性 积 
Izy =I yz = =] xydm (11-134) 
B 
Ls = Taye =| yzdm (11-135) 
B 
I= =- | zx dm (11-136) 
B 


刚体 的 惯性 矩阵 工 中 的 元 素 由 离散 的 质点 构成 ， 在 式 (11-50) 中 进行 了 定义 。 
工 中 的 元 素 是 绕 着 建立 刚体 质心 C 的 刚体 坐标 系 的 惯性 矩 。 因 此 工 是 一 个 独立 于 坐标 系 
的 数 ， 并 且 必须 像 ? 一 样 书写 ， 以 表明 它 被 计算 时 所 在 的 坐标 系 。 

















y? te? =y — zz 
"r= | — zy z? +r? — yz dm= | GI—rr dn = | —Frrdn 
B B B 
一 之 万 —yz a? +y? 


(11-137) 
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惯性 矩 可 以 从 一 个 坐标 系 Bi 转换 到 另 一 坐标 系 Bo 中 ， 依 据 旋 转轴 定理 这 两 个 坐标 系 
均 需 建立 在 刚体 质心 处 
B= Rp "IPRS, (11-138) 
依据 平行 轴 定 理 ， 将 位 于 矢量 “rc 处 的 中 心 坐标 系 Bi 中 的 惯性 矩 转 换 到 平行 于 坐标 系 
Bi 的 另 一 坐标 系 B: 中 ， 有 


K 














y=] tm Fo Fo (11-139) 
如 果 定 位 局 部 坐标 系 Ozyx 以 致 其 惯性 积 消失 ， 那 么 局 部 坐标 系 就 被 称 为 主 坐 标 系 ， 有 
关 的 惯性 和 矩 被 称 为 主 惯性 矩 。 主 轴 和 主 惯性 矩 可 通过 求解 下 列 方程 中 的 工 获得 。 








Ia I Ty ia 
foe. Ig- I Doe (|=0 (11-140) 
Iex Iy Ial 
[Aj — Co, ) | =0 (11-141) 
因为 式 (11-141) 在 工 中 是 一 个 3 次 方程 ， 因 此 我 们 可 获得 如 下 3 个 特征 值 
L=I, Ts=I, =l, (11-142) 


上 述 特征 值 是 主 惯 性 矩 。 
我 们 可 以 利用 章 次 位 置 矢 量 并 定义 一 个 广义 惯性 矩 ， 被 称 为 伪 惯 量 和 矩阵。 


| x? dm | xydm 
B B 


Jasin [xd 
| 
jem | 
































ira] rr dm= (11-143) 
B | xz dm f yzdm z dm 
B B B B 
| xdm | ydm | zdm | dm 
B B B B 
这 个 伪 惯 量 和 矩阵 可 以 扩展 为 
站 
I ry I zz MEC 
2 ~ 
I Ly — Tyy File I 
Te i 2 í — (11-144) 
I I I xy sil I Yy Bz 
iz zy 2 MZC 
MXC MYC MZC m 
这 里 
| xdm 
mp 
TCE 
B 1 
rc=|yc |= 1f ydm (11-145) 
mJB 
zc 
二 | zdm 
mJB 








式 (11-145) 中 的 矢量 是 局 部 坐标 系 中 质心 的 位 置 。 如 果 刚 体 坐 标 系 是 中 心 坐标 系 ， 那 么 这 
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个 矢量 是 零 。 

证 明 : 

刚体 质心 处 具有 共同 原点 的 两 个 坐标 系 如 图 11-5 所 示 。 通 过 矢量 变换 法 则 ， 将 来 自 坐 
标 系 Bi 的 刚体 角速度 和 和 角 动 量变 换 到 坐标 系 Bo 中 。 

















oR "o (11-146) 
L=" Rp, BL (11-147) 
然而 ,根据 式 (11-148) L Flo 是 关联 的 。 
BT =B pig (11-148) 
因此 ， 有 
L=™Rp, BRT Beg =P I ø (11-149) 
st (11-149) 表明 了 如 何 将 来 自 坐 标 系 Bi 的 惯性 矩 转换 至 一 个 旋转 坐标 系 Bs 中 。 
2 y=hR, MI™RT (11-150) 
图 11-5 刚体 质心 处 具有 共同 原 图 11-6 中 心 坐标 系 Bl 和 变换 坐标 系 Be 











点 的 两 个 坐标 系 








现在 考虑 位 于 “rc 处 的 一 个 中 心 坐 标 系 Bi. WE 11-6 所 示 ， 该 坐标 系 围绕 固定 坐标 系 
Bo 原点 旋转 ， 从 而 使 它们 的 坐标 轴 保 持平 行 。 通 过 下 式 将 来 自 坐 标 系 Bi 中 的 刚体 角速度 和 
角 动 量 转换 到 坐标 系 B 中 。 











> o=" o (11-151) 
L = L4 (ro Xm vc) (11-152) 
因此 ， 有 
L =L mrX aX) =P Ltn rr veo =O rtm” re ree 
(11-153) 
式 (11-153) 说 明了 怎样 将 来 自 坐 标 系 Bi AY EERE HS IRB POP AT AY A A Bo 中 。 
I=™I+mrcert (11-154) 
该 平行 轴 定 理 也 被 称 为 惠 更 斯 -斯 坦 纳 (Huygens-Steiner) 定理 。 














为 了 将 惯性 矩 转 换 到 一 个 旋转 坐标 系 中 ， 参 考 式 (1-150) 我 们 总 可 以 找到 一 个 坐标 
系 ， 在 该 坐标 系 中 “I 是 对 角 线 矩阵 。 


> Rp, OR. (11-155) 


413 


和 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





或 者 
J 
Iya 
Ter Ia Ia 
式 (11-156) 表明 Ii, Te. 


六 3 r32 
| A 
I yx 
I zx 


Ts PT WEA 


r33 





La 
Iyy—A 
T 


I; 0 O\(ri riz ri 
O T O |lra rz rz (11-156) 
0 0 了 3 r31 r32 T33 


FE 值 。 通 过 求解 下 列 方程 可 得 到 这 些 特征 值 。 
Iss 
T yz 
一 从 


=0 (11-157) 


I 


RIEL Ti. L 和 了 是 主 惯性 矩 ， 其 有 关 特 征 矢量 被 称 为 主 方向 。 由 特征 矢量 所 构成 的 
坐标 系 是 主 刚体 坐标 系 。 在 主 坐 标 系 中 ， 刚 体 角 动量 是 














Li L 0 0\)fwi 
Ls|=|0 Te 0 lw; (11-158) 
Ls 0 0 I3)\ws 
例 11-12 EREE. 
考虑 惯性 和 矩阵 
20 一 2 0 
T 一 | 一 2 30 0 (11-159) 
0 O 40 
依据 式 (11-141) 建立 行列 式 
20 一 A —2 0 
一 2 30-Aa 0 |=0 (11-160) 
0 0 40—A 
可 得 特征 方程 为 
(20—A)(30—A) (40—A) —4(40—-A) =0 (11-161) 
式 (11-161) 的 3 个 根 为 
11=30.385 Is=19.615 I3=40 (11-162) 
因此 ， 惯 性 矩阵 的 主 惯性 矩 为 
30. 385 0 0 
I=| 0 19.615 0 (11-163) 
0 0 40 
例 11-13 EKRA. 
考虑 惯性 矩阵 
20 一 2 0 
T 一 | 一 2 30 0 (11-164) 
0 O 40 
通过 求解 式 (11-165) 可 建立 主轴 x; 的 方向 。 
了 Lzy 1 xe cOSQ ; 0 
Ijz Lagat Tye cosB,; |=] 0 (11-165) 
Teg Tey Te—I:)leosy;) (0 
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对 于 方向 余弦 ， 式 (11-165) 也 必须 满足 











cos*a; +cos? f; Hcos? y, =1 (11-166) 
对 于 第 1 EMM Iı =30. 385, A 
20—30. 385 —2 0 cosa 0 
—2 30—30. 385 0 cosB1 |=] 0 (11-167) 
0 0 40—30. 385) cosy 0 
或 者 
一 10. 385cosal 一 2cosp1 十 0 一 0 (11-168) 
一 2cosal —0. 385cos8, +0=0 (11-169) 
0 十 0 十 9. 615cosy; =0 (11-170) 
求解 式 (11-168) ~sk (11-170) 可 得 
a1=79.1” B=169.1” y, =90. 0° (11-171) 
对 于 第 2 主轴 使 用 12 二 19. 615， 有 
20—19. 62 —2 0 cosa? 0 
—2 30—19. 62 0 cos |=] 0 (11-172) 
0 0 40—19. 62} eosy,) (0 
可 得 
as =10.9° By=79.1° 7y,=90.0° (11-173) 
对 于 第 3 主轴 使 用 Ts 一 40， 有 
20 一 40 一 2 0 cossus 0 
—2 30—40 0 cosB, |=] 0 (11-174) 
0 0 40-40) eosy,} \0 
可 得 
a3=90.0° B,=90.0° y¥3=0.0° (11-175) 
例 11-14 HE EH EF BY PE 。 
Bike Am, REAL. HEN w, BEEN A 
的 齐 次 矩形 连 杆 ， 如 图 11-7 所 示 。 R 
局 部 中 心 坐 标 系 建 立 在 连 杆 的 质心 处 。 利 用 积分 方 ” l > 
法 可 求 得 连 杆 惯性 矩阵 的 惯性 矩 。 我 们 从 计算 Ice FP baa x 
Ba CP +zddm= | GP +e2)pde= | 图 11-7 齐 次 矩形 连 杆 
m [h/2 fw/2 ri/2 ii 
Gt) d=" | | | (y? +z? )dadydz == (w? +h?) (11-176) 
lwh J—n/2 J —w/2 J —1/2 12 
类 似 地 ， 也 可 以 计算 Tv A Tex» 
Lo (11-177) 
lies (Pa) (11-178) 


因为 该 坐标 系 是 中 心 坐 标 系 ， 因 此 惯性 积 必 须 为 零 。 为 了 说 明 这 个 ， 我 们 验证 Icy 。 
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. m fh/2 fw/2 pl/2 
fay =| zydm= | Zyo dm 一 | | | ydrdydz=0 (11-179) 
B v lwh J—n/2 J —w/2 J —/2 
因此 ， 刚 性 和 矩形 连 杆 惯性 矩阵 的 惯性 矩 在 其 中 心 坐标 系 中 为 
m 
OP Geo Neg e 
126% he) 0 0 
m, 
I= 0 pi +e?) 0 (11-180) 
Meg ,2 
0 0 12°! +w?) 


例 11-15 ”惯性 矩阵 的 平移 。 

TEE RA BCozyz) 中 ， 刚 性 连 杆 (图 
11-8) 惯性 矩阵 的 惯性 矩 在 式 〈11-180) 中 
已 给 出 。 在 非 主 坐标 系 中 ， 惯 性 矩阵 的 惯性 
抢 可 通过 应 用 平行 轴 变 换 公 式 [ 式 (11- 
154) | 求 得 。 

Bp Prt ne PFE (11-181) 
































质心 位 置 矢 量 为 
L 
"r= jw (11-182) 图 11-8 在 主 坐标 系 和 非 主 坐标 系 中 
h 的 刚性 矩形 杆 
因此 ， 有 
—h w 
"Fe 一 也 h o 一 (11-183) 
一 也 L 0 
由 式 (11-183) 可 得 
hem +i wèm — mw -+ him 
3 了 一 — mw Shem +m -E imw (11-184) 
= hem = Thmw zl? EI wm 
例 11-16 We Fe EK 
假设 刚体 惯性 矩阵 为 
2/3 一 1/2 —1/2 
了 一 | 一 17/2 5/3 —1/4 (11-185) 
—1/2 —1/4 5/3 
T 的 特征 值 和 特征 矢量 为 
2.351 
I, =0. 2413, 1 (11-186) 
1 
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—0. 851 
I,=1.8421,; 1 (11-187) 
1 
0 
Ts 一 1.9167,| 一 1 (11-188) 
1 
标准 化 特征 矢量 矩阵 W 等 于 形成 惯性 对 角 线 矩阵 的 变换 矩阵 的 转 置 和 矩阵。 
0.8569 一 0.5156 0 
W=(W, W: W;)=?R; =| 0.36448 0.60588 —0. 70711 (11-189) 
0. 36448 0.60588 0. 70711 
可 以 验证 : 
0.2413 一 1X1071 0. 00 
I= R PR] =W IW=|—1X1074 1.8421 —1X 1079 (11-190) 
0.0 0.0 1. 9167 


We GY 11-17 ”相对 的 对 角 线 惯性 和 矩 。 
利用 惯性 和 矩 的 定义 [ 式 (11-131)、 式 (1-132) 和 式 (11-133)]， 我 们 可 以 知道 惯性 矩 
阵 是 对 称 和 矩阵 ， 并 且 有 














| (C22 + yt te?)dm=3 (Tee HIyy HIz) (11-191) 
Lrs HI yy >I z (11-192) 
Ty FIzz >I cee (11-193) 
Ti Tg Tyy (11-194) 
注意 
(y—z)’ 宇 0 (11-195) 
显然 有 
y? 十 z2 之 2yz (11-196) 
因此 ， 有 
Tx 2T yz (11-197) 
类 似 地 ， 也 有 
Iyy S2] zx (11-198) 
I >2I ry (11-199) 
WK 11-18 ”特征 方程 的 系数 。 
由 式 (11-157) 知 
amà Ia Iz 
la Ip A J |=0 (11-200) 
了 zz Egy Taga 


HTA EREE, EK (11-200) 展开 成 一 个 有 关 4 的 3 次 方程 ， 称 为 特征 
方程 。 





à? —aià? +HazàÀ—a; =0 (11-201) 
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特征 方程 的 系数 被 称 为 工 的 主要 不 变量 。 特 征 方程 系统 从 下 列 方程 中 可 以 直接 求 得 : 























a1=Tzz Tyy tT =tr(I) (11-202) 
a2 = Tox T yy | I yy Izz | Tal zz IÈ, IER I, 
los Lay Ila daz Tix Tye I , (11-203) 
E =— [aĵi trQ’) ] 
Ty Iy Izy Iz l Les 2 














4&3 7 Izz Las I zz T Izy Lyg Tex F Teyl yx Izz (Lax I yz Izy + I yy Tz, Izz F Iz Izy I yx ) 
SI pr] yyl FI ey] yel ex — (TT HI yy leg teed, = l 


zz" xy 





(11-204) 








KY 11-19 ETE READ hp AE E 

Tae ERP IE Ty Be AY AB ae EE. IP AI ASR. (AE AR A ABTA]. SE TEE E E Be 
fA. BU EEE ERR eT OIA Ia A eM A. A ST; 的 最 小 值 和 最 大 值 并 不 取决 
于 刚体 坐标 系 ， 特 征 方程 的 解 并 不 取决 于 坐标 系 。 

eM, WHT. I ATs ÆI EREE, BAP TH" Re IPR PT AY 
EREE In To 和 Ts。 

我 们 得 出 结论 : Ti. L 和 Ts 是 惯性 矩阵 工 的 坐标 不 变量 。 因 此 任何 取决 于 五 、T2 和 
Ts 的 数值 也 是 坐标 不 变量 。 和 矩阵 了 唯一 有 3 个 独立 不 变量 ， 并 且 每 个 其 他 不 变量 可 以 基于 
Li, I2 和 了 3 进行 表述 。 

FI Iis I2 和 Ts 是 矩阵 工 特征 方程 的 解 ， 见 式 (11-201)， 我 们 可 以 用 下 列 形式 写 出 
式 (11-157)。 
























































QA—I1)Q—12)—13)=0 (11-205) 
该 方程 的 扩展 形式 为 
X43 一 (I 十 Jz 十 Ta3)42 十 (IT1T2 十 Tas 十 TaT1)4 一 T1213 二 0 (11-206) 
比较 式 (11-206) 和 式 (11-201)， 我 们 得 出 结论 : 
ai= lse FI tlæ=IitIz+I; (11-207) 
t= lel gel zz +Izr I2, I 5I I+II; +III (11-208) 
as =I rsl yyl zz 2 asl yzl Url HI yya HIIS IIs (11-209) 


表述 作为 L, I: MIs RAIRA ai, az 和 as， 可 以 判定 特征 方程 的 系数 是 坐标 不 


i 
f 


KBI 11-20 ”惯性 矩阵 元 素 的 简短 标记 。 
HAP Ae 函数 [ 式 〈2-201)]， 我 们 可 以 简短 形式 写 出 惯性 矩阵 I 的 惯性 矩 中 的 





元 素 。 

| [Cri Hri tri) —xix; dm (11-210) 
B 

B= (r26i; — zix; dm (11-211) 
B 

3 

w=] [Zu — xij) din (11-212) 

B 


k=1 








这 里 ， 我 们 可 以 利用 下 列 标记 。 
X1=X Z2 一 yy X3=z (11-213) 
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Sis min M 





* Gi 


11-21 




















相对 于 平面 、 直 线 和 点 的 惯性 矩 。 


相对 于 平面 、 直 线 或 点 定义 微粒 系统 的 惯性 矩 : 微粒 质量 乘 以 微粒 距 平 面 、 直 线 或 者 点 
的 垂直 距离 的 平方 之 和 。 对 于 连续 的 物体 ， 该 和 是 对 整个 物体 体积 的 有 限 积分 。 





























相对 于 zy 平面 、yz 平面 和 xz 平面， 惯性 矩 为 
w=] z? dm (11-214) 
B 
12 | x? dm (11-215) 
B 
Ty =| y? dm (11-216) 
B 
相对 于 xz 轴 、y 轴 和 > 轴 ， 人 惯性 矩 为 
| Cy? 十 z?) dm (11-217) 
B 
=l (Cz? +x? )dm (11-218) 
B 
| Ca? + y? dm (11-219) 
B 
因此 ， 有 
了 二 (11-220) 
Ty 一 了 zz 十 了 zs (11-221) 
I,=I,2+T1ye (11-222) 
A XT Fu. EEEN 
1 
| (xz? 十 y? 十 z2)dm 二 Tz 十 1 yi 十 ;2 二 2 Cael yi) (11-223) 
B 


由 于 坐标 系 的 选择 是 任意 的 ， 因 此 我 们 可 以 说 : 相对 于 直线 的 惯性 矩 是 相对 于 任何 两 个 
互相 垂直 且 通 过 直线 的 平面 惯性 矩 之 和 。 对 于 3 个 互相 垂直 且 相 交 于 一 点 的 平面 来 说 ， 相 对 
于 点 的 惯性 矩 也 有 类 似 的 含义 。 





























11.5 牛顿 运动 方程 的 拉 格 朗 日 形式 


牛顿 运动 方程 可 以 被 转换 成 














d(?K ) aK 
| A | F, r=1,2,…,n (11-224) 
délag,} 94: 
qr 
这 里 
< Ifi Ig; Ih; 
= : | | = 
F, > (ra HP iy a HFa) (11-225) 


式 (11-224) 称 为 拉 格 朗 日 运动 方程 ， 其 中 KK 是 nn 自由 度 系 统 的 动能 , gq; (~ 王 1，2，…， 
n) 是 常规 的 坐标 系统 ，F; 二 (Fi;。 Fi Fiz)’ 是 施加 系统 中 第 i 个 微粒 上 的 外 力 ，F, 是 与 
dr 关联 的 力 。 

证 明 : 

假设 m: 是 一 个 系统 微粒 中 的 一 个 微粒 的 质量 ， 并 且 假 设 (x;，y;，zi) 是 固定 全 局 坐 
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标 系 中 的 笛 卡 儿 坐 标 。 假 设 每 个 微粒 的 坐标 都 是 另 一 组 坐标 91 ，g2，g3， 


时 间 z 的 函数 。 























ets gn 和 可 能 的 



































Xi=fi(g1,92,43.°"" Gn ot) (11-226) 
Yi = gi (G1 42543 Gn ot) (11-227) 
zi =h; (gi +g2+93-% dn ot) (11-228) 
WR Fa, Fy, Fei ete phim, 上 合力 的 分 量 ， 那 么 该 微粒 的 牛顿 运动 方程 将 为 
Foi =m; 2 (11-229) 
Fyi=mi yi (11-230) 
Fi =m; zi (11-231) 
我 们 给 这 些 方程 的 两 边 分 别 乘 以 式 (11-232) ~} (11-234) 
af; 
二 (11-232) 
adr 
Igi 
> (11-233) 
Iq, 
dh; 
(11-234) 
dqr 
对 于 所 有 微粒 ， 将 其 求 和 ， 则 有 
$ aw Of ge IBY aha z Ifi Igi sti) 
i| xz; by; Hz; =>] Fu; HFyi HFa (11-235) 
a4” ¢ I ae SI “9g, Yog, 4q, 
这 里 ,nn 是 微粒 总 数 。 
对 式 (11-226) 求 时 间 导 数 ， 有 
afi. Off. Ife: Ifi. fi 
ea | ks | | 上 Zs 
ğa Jg 1 das 1 Iga T j agd” mer (11-236) 
可 以 发 现 
ax; 9 Ifi. Of;. Of;. Ifi. Of ; af 
a a a eg tae (11-237) 
Ig, 3q, \?4ı 9g2 9g3 Ign dt Iq, 
因此 有 
afi z Izi d(. Izi d Ixi 
3 i Ti E = Ti 一 vri (11-238) 
adr adr de dgy de dgy 
d(a%:)_. d(afi)_. P Pf, Pf, Pfi. Pf: 
zi gho |2 glo. [7% gid ger ae qn 十 一 一 
i dqr t dqr 9q19gr 9q29gr 9qg39gr 9qn9gqr 9t9gqy 
Qf is Sis afi. If: . ORY 
a bsna ad =z; (11-239) 
* a L Bo a ge 
并 且 有 
.. 3zi df. Az; Ixi 
zi 一 -一 Zi 一 i 一 (11-240) 
ag, UN ag, ag, 


st (11-240) 等 效 于 
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w On; df 4 fl. 3 . 
ees a l : Gal - (#7) (11-241) 
adr t dq, adr 2 


现在 ， 将 式 (11-238) 和 式 (11-241) 代入 式 (11-235) 的 左边 ， 则 有 


Z as of ; ee Og; Ah; 
mil x; Fyi rz 
aq, 


Si dqr Iq, 
































Bh de Iq, = Iq, 
— 42K 93K 
di9g, Iq, 
(11-242) 
这 里 
D (23 +y? +23) (11-243) 


i=l 


xt (11-243) Pere 因此 牛顿 运动 方程 [ 式 (11-229), si (11-230) 和 
式 (11-231)] 可 被 转换 成 


daK dK .. 9f; Igi .. Oh; af; ag; dh; 
mn 一 mal a | eee 一 . Sa . ce £ 
dt aq, aq i 之 Mi | Ti Iq, Yi Iq, Tz ‘gg, 之 Fi | Fyi ， T Fz > 

















(11-244) 
因为 式 (11-226), xe (11-227) 和 式 (11-228)， 因 此 动能 是 ol a2. qas s dn 和 时 间 
t 的 函数 。 式 (11-244) 左边 包括 整个 系统 的 动能 ， 右 边 是 常规 力 并 且说 明 从 x; Bq; 的 变化 
坐标 对 外 力 的 影响 。 假 设 坐 标 g; 改变 到 ,十 bo, ， 与 此 同时 其 他 坐标 gi1、g2、g3、*…、gr-1、 
Grtis tts Qn 和 时 间 z 没有 改变 。 因 此 ， 微 粒 m: 的 坐标 变化 为 
Ifi 

















x; tz òq, (11-245) 
Iq, 
Igi 
Yit Ogr (11-246) 
adr 
dh; 
SeT Ogr (11-247) 
Iq, 
a a E i 
i a a 
SW = > (rast ae 3 joa (11-248) 
i=l "Og, dq 











因为 内 力 所 做 的 功 成 对 地 出 现 ， ee rere 在 式 (11-248) 中 。 让 我 们 
用 式 (11-249) 表示 虚 功 。 











OW=F,(g1,g2,g43,°""* ,qn st 0gr (11-249) 
这 时 有 
aK 9 
a z | LN (11-250) 
dt dqr dq, 
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这 里 


Re 





式 (11-250) 是 拉 格 朗 日 运动 方程 。 
二 阶 微分 方程 ， 在 该 方程 中 qi, 
“dn 被 称 为 广义 坐标 ， 并 


An^ 
qis G2. G3. 
和 相关 变量 数 是 相等 的 ， 在 理 Y 





(11-251) 


这 个 方程 适 应 于 /的 所 有 值 ， 从 1 到 nnn。 这样 我 们 

gd3、 e qn 是 关联 变量 ， t 是 独立 变 SH 坐标 
且 是 可 测量 的 参数 以 提供 系统 组 态 。 因 为 方程 数 
伦 上 方程 可 充分 确定 





i=l 











d2. 





所 有 微粒 质量 m; 的 运动 。 
例 11-22 Aye, 


单 摆 如 网 11-9 所 示 。 利 用 质量 mF FA BL iz 


置 的 坐标 x Ally 
x= f(0)=lsind ( 


y =g (0) =lcosd ( 





: : 1 。 
ma? +y?)= ml? ® ( 


1 
K 2 


因此 ， 有 
aK 


并 将 0 =q 作为 一 般 坐标 有 


11-252) 


11-253) 


11-254) 


d 





a (aK 
dt ab 90 


施加 到 质量 m 的 外 力 分 量 为 





因此 ， 有 


~ di 





单 摆 的 运动 方程 为 
ml? 0 =—meglsind 


例 11-23 连接 到 振荡 质量 的 单 摆 。 
图 11-10 所 示 为 带 有 单 摆 的 振动 质量 块 。 


(11-259) 

















如 果 正 


确 设 计 单 摆 ， 那 么 单 摆 可 以 作为 一 个 振动 吸收 器 用 。 


从 坐标 关系 开始 
XM=fM=Zx 
yM 一 3SM 一 0 
Lm =fm=xtlsind 
Ym =E m =Llcosd 
我 们 可 以 依据 广义 坐标 x 和 0 求 得 动能 
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(11-260) 
(11-261) 
(11-262) 
(11-263) 


(ml26 y=ml? g 


=—mglsinð 








图 11-9 








单 摆 








Kl 11-10 4 


R BM WN aie 


(11-255) 


(11-256) 
(11-257) 


(11-258) 
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1 : : 1 : è 3 5 : . 
K=Fmm (rutyM) Hz Mm Ga? Hya) S= mmr? Hm pn (re? +1762 +2126 cosh) 


2 


这 时 ， 拉 格 朗 日 方程 的 左边 部 分 是 

















d(9K\ oak . z TE 
a 5 = (my tmm) £ +mnlO cos0—m nl * sind 


dt | az Ix 
d(9K) aK . z 
| =M ml? 0 +m,,l x cosh 
dt | ag 30 
作用 到 M Am 上 的 外 力 为 
Fom Rx 
F yy =0 
Fon =0 
Fy =M m8 
因此 ， 广 义 力 为 
afm IgM IS m IZ m — 
F a F TM ax YM Ix F Tm ax Ym Ox kx 
afm Ig mM If m IZ m } 
F,=F,m 20 Py 20 Pigs, 20 Fv, 90 =—my,»glsind 











最 后 ， 拉 格 朗 日 运动 方程 为 


(mm EMm) £ Emml 0 cos0 一 7 „16 2 sind = —kax 





Mml? 0 mml x cosh = —m mglsin 


Ww 11-24 HY. 
如 果 一 个 m: 质量 系统 正在 一 个 力 场 中 移动 





那么 它们 的 牛顿 运动 方程 将 为 
mir; =—V:;V 1 一 1,2，…，,71 
运动 方程 与 7; 内 积 ， 并 且 将 所 有 方程 求 和 有 


n n 


>X m: r; n r:=— ` ri ba Viv 


i=1 i=1 





这 时 ， 对 时 间 积 分 
1< : 5 i 
z > mir;i; “ri 一 [Stew 


i=] i=] 





st (11-278) 表明 





(11-264) 


(11-265) 


(11-266) 


(11-267) 
(11-268) 
(11-269) 


(11-270) 


(11-271) 


(11-272) 


(11-273) 


(11-274) 


(11-275) 


(11-276) 


(11-277) 


(11-278) 


(11-279) 
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XE, E 是 积分 常数 。E 被 称 为 系统 机 械 能 ， 等 于 动能 和 势能 之 和 。 
例 11-25 ”地球 的 动能 。 
地 球 是 一 个 绕 着 固定 轴 旋 转 的 近似 刚体 。 地 球 的 两 种 运动 分 别 是 绕 着 太阳 的 公转 和 绕 着 

















近似 固定 在 地 球 内 部 的 一 条 轴 自 转 。 由 于 地 球 的 自转 ， 因 此 地 球 的 动能 》 
了 (6356912 十 6378388Y21 2x 366. 252 
armyan 9742x100 2 ) an eo) 


=2. 5762 X 109J (11-280) 
由 于 公转 ， 








1 Qn 1 \? 
K: => 一 二 (5. 9742 X 1024 75X10" 
p= ma 66. 9742 X 1024) (1. 49475 X 10")? Gee J 


三 六 ——- (11-281) 
这 里 , r 是 地 球 距 太阳 的 距离 ，w1 是 绕 着 地 球 自 身 轴 的 角速度 ，w* 是 绕 着 太阳 的 角速度 。 
地 球 的 总 动能 是 KK 二 Ki 十 K，。 然 而 ， 公 转动 能 对 自转 动能 之 比 为 
Kə 2.6457X1033 
Kı 2.5762X102 
Ww Gl 11-26 ” 非 笛 卡 儿 坐 标 系 统 。 
抛物 线 坐 标 系 和 第 卡 儿 坐 标 系 通过 式 〈11-283) 一 式 (11-288) 相关 联 。 








10000 (11-282) 














x = Ecos (11-283) 
y = 7ésing (11-284) 
2p? 
ga (11-285) 
2 
C= Ja? +y? +z? +z (11-286) 
7 二 Vx 十 y ?十 ?一 之 (11-287) 
加 y 
p=arctan 一 (11-288) 


在 均匀 电场 中 沿 着 x 轴 正 方向 的 一 个 电子 由 于 原子 核 而 处 在 中 心 引力 场 的 作用 下 





k k 
下 一 一 二 6， 一 | ) (11-289) 
2 


在 原子 内 部 均匀 电场 会 影响 电子 的 运动 ， 这 种 影响 被 称 为 斯 塔 克 效 应 (Stark effect), 
mer eee ne caer ee Zz, 
电子 在 抛物 线 系统 中 的 动能 ; 






































K= mG? Fy? 十 2 ) 一 =m Cp FENG HED Hy ep? | (11-290) 
作用 到 电子 上 的 力 为 
k 2k œE 
F= ví r Herz) v{ Py 5 E 0 j (11-291) 


式 (11-291) 可 产生 下 列 的 广义 力 
4A7 i 
(2477)? re 





F,=F +b, 





(11-292) 
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ARE 
(62 十 772 )2 
=0 





F:=F ° b: 





eEn 





这 里 ，bs b, Mb, 是 坐标 系 中 的 基体 矢量 。 


or 、 网 
b: = a ncosgi + nsingj 十 全 








or . A n 
i Oe Esingj + yk 





or | 
Ig 
因此 ， 按 照 拉 格 衣 日 方法 ， 电 子 的 运动 方程 为 


b, = 一 Msinpi 十 cospjy 


2 


F= mq G+ 92 my G+ Egg" 








d : ; : : 
Fea ling E + a) l= may? + E2) — mén? p? 


d : 
Fes gE 9 p) 
Ww 11-27 拉 格 朗 日 方程 的 显 式 形式 。 





(11-293) 


(11-294) 


(11-295) 


(11-296) 


(11-297) 


(11-298) 


(11-299) 


(11-300) 


假设 每 个 微粒 的 坐标 是 坐标 ol 、g ga. ttt. an 的 函数 ,但 不 是 时 间 +t Mew Be. H n 


个 质量 微粒 所 构成 系统 的 动能 可 以 被 写成 
=> X matti tD Sa, 
i= 


j=lre=l1 


里 ， 系 数 ajy 是 qi1、g2、g3、…、g 的 函数 ， HEHEA 


ajk apj 


d (dK aK 
7 =F, r=l,2, „n 
dt aq, dq, 





拉 格 明日 运动 方程 











这 时 ， 等 于 
LON ar. . 
E Dawin y > EdF, 


m=i j=lk=l Agr 
或 者 


n n 


ee ee: Sr alid = 


m=1 j=1k=1 


RE, T 被 称 为 克里斯托弗 尔 算 子 (Christoffel operator), 


i l /9ai Jaik Ian; 
Y= = 
2\9g 9g; qi 








11.6 拉 格 朗 日 力学 


假设 对 于 某 些 力 F 一 (Fi。 Fy Fe)T7， 有 一 个 被 称 为 势能 的 函 


RV 


(11-301) 


(11-302) 


(11-303) 


(11-304) 


(11-305) 


(11-306) 


ws: 运动 学 、 动 力学 与 控制 技术 





势能 导出 的 。 
F=-—VV 
这 样 的 一 个 力 被 称 为 位 势力 或 者 保守 力 。 这 时 拉 格 明日 运动 方程 可 写 为 


d( 3IL OL _ 
dt\ag,} ar Re Peen 
































这 里 
f=K—V 
是 系统 的 拉 格 朗 日 函数 ，Q, 是 非 潜 在 的 广义 力 。 
WEAR: 
假设 作用 于 系统 的 外 力 下 = (Fi, Fiy Fa)" 是 保守 力 。 
F=—-VV 
这 些 力 在 任意 虚 位 移 6g1 ，6g2 ，6g3，…，6g» 上 所 做 的 功 为 
aV OV aV aV 
IW 5 Bae aos aay 
这 时 ， 拉 格 明 日 方程 变 为 
d (9K aK 3V 
ce P oe r=1,2,°,n 
引入 拉 格 朗 日 函数 “一 氏 一 多 ， 则 对 于 保守 系统 ， 拉 格 衣 日 方程 转变 为 
A) = 0 r=1.2,°4n 
dż ” dr 


拉 格 朗 日 函数 也 被 称 为 动 势 (kinetic potential) 。 
如 果 一 个 力 不 是 保守 力 ， 那 么 由 该 力 所 做 的 功 为 


> Of i Jg; dh; 
一 | | | A | 
OW a (Fe PF T Fy a CF ,; Naa, Q,0q, 
并 且 运 动 方程 将 为 


df IL) ag 
dz J; Jq =Q, = sn 
3q, dqr 


这 里 ，Q, 是 非 潜 在 的 广义 力 ， 该 力 在 第 7 个 广义 坐标 g, 的 一 个 虚 位 移 上 做 功 。 
例 11-23 RIE, 























利用 钟 摆 类 比 对 许多 动力 学 问题 进行 建 模 。 图 11-11 所 示 为 具有 质量 m 和 长 度 / 的 





(11-307) 


(11-308) 


(11-309) 


(11-310) 


(11-311) 


(11-312) 


(11-313) 


(11-314) 


(11-315) 








球 摆 。 用 角度 p 和 0 描述 系统 坐标 。 
质量 块 作 为 广义 坐标 的 函数 ， 其 笛 卡 儿 坐 标 为 
X rcososinð 























Y |=|rsingsinð 
Z —rcosl 
因此 ， 单 摆 的 动能 和 势能 ; 
1 : : 
K= jm (1202 +1? p? sin’) 
V=—~meglcosé 
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个 


(11-316) 


(11-317) 


(11-318) 


Sis min M 





a 日 函数 为 
f= =m (1202 十 12p2sin20) +mglcos6 


(11-319) 
由 式 (11-319) 可 得 下 面 的 运动 方程 。 





0 — o° sinf cos + + sind = Q (11-320) 


¢ sin20+2¢0 sindcosd =0 (11-321) 
例 11-29 单 连 杆 机 械 手 。 
一 个 单 连 杆 机 械 手 如 图 11-12 所 示 。 假 设 
在 关节 处 存在 黏 性 摩 探 ， 在 关节 处 一 个 理想 
电动 机 可 将 转 矩 Q 作用 到 机 械 辟 上。 理想 电 
动机 的 转子 并 没有 惯性 和 矩 。 
机 械 手 的 动能 和 势能 




















图 11-11 BRP 


iL 1 ; 
K=510°=7 Uc tml? )0 ? (11-322) 


V=—meglcosé 





(11-323) 


这 里 ，m 是 质量 , I EPRE O 点 的 惯性 矩 。 机 械 手 的 拉 格 朗 日 函数 为 


Y=K-V= 了 10 tmglcos? (11-324) 








因此 ， 机 械 手 的 运 es 











~ dt 20 


ag 





d(I) av n l 
I@+meglsin@ (11-325) 


广义 力 M 是 电动 机 转 矩 Q AN Bi YE RE PRM J Hc 


的 贡献 。 因 此 ， 机 械 手 的 运动 方程 为 


Q=I 6+c@ +megl sind (11-326) 


例 11-30 理想 2R 平面 机 械 手 动力 学 。 














2R 平面 机 械 手 的 理想 模型 如 图 11-13 所 示 。 它 被 称 





为 是 理想 的 ， 因 为 我 们 假设 连 杆 是 无 质量 的 ， 并 且 
无 摩擦 。 质 量 m! 和 ms 是 驱动 第 2 根 连 杆 的 电动 机 
质量 和 末端 点 处 的 负载 。 我 们 用 绝对 角 0 和 相对 角 
92 作为 广义 坐标 表达 机 械 手 组 态 。 
质量 m Mm: 的 全 局 位 置 撩 量 为 

Xı lı cos 

i en ere 
Xe) lı cosi +l2cos(0; +02) 
k Pea +12 sin(@; +02) 
因此 ， 质 量 m1 Mm: 的 全 局 速度 矢量 为 





























(11-328) 











Q, 





Q 








图 11-12 单 连 杆 机 械 手 











=X 








Al 11-13 2R 平 面 机 械 手 的 理想 模型 
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Xi 一 101sin01 

-| | TE 
Y lı0ıcosĝı 

X2 —/L101sin01—/L2 (01 #0 2)sin(01 +82) 

é -| | (11-330) 
Y 110, cos@, +l (01 +02)cos(0ı +02) 


机 械 手 的 动能 由 质量 mi 和 me 的 动能 所 构成 ， 即 


1 . 1 . . 
K=K,+K2= smi (XH PY. al zm (X ee 

















= m i+ m2 (036? 1 72(6,+62)2+2111261(61+62)cosh2] (11-331) 
机 械 手 的 势能 


V=V) 十 V2 =migYi 二 m2gY2 =migli sind, t+mogLlh sing 十 12 sin(01 +02 il 


(11-332) 


这 时 ， 拉 格 朗 日 函数 从 式 (11-331) 和 式 (11-332) 中 可 获得 。 





1 : š soi ; 
zmıli 6? “he ee small 624 +13 (01 +02)? +2111201 (01 +62) cose | 




















L=K 一 V 一 到 
一 (zlgllisin0l 十 72gLLisin0l 十 02sin(O1 +42) J} (11-333) 
对 式 (11-334) 求 偏 导 ， 有 
IL _ 
J9, — (mı +m2)glı cosi —m2gl2cos(@1 +02) (11-334) 
ay $ f 3 A 5 
= =(m; +m2)1201+m213 (6, +62) +meol 112 (201 +62)cosb2 (11-335) 
90 1 
alL T T + 
=| 一 二 (mi 十 m2)1?01 十 m2l12(01 十 92) 十 
di (30; 
malila tor OI C000 pasa 8 aes (11-336) 
ay ee , 
3g, m2titaO1 (01 1 G2 )sind, —mzglacos(91 +82) (11-337) 
aL 
=mz213 C01400) mal ilz e080 (11-338) 
30 
(11-339) 


aa? sis wi si AE 
=a EET =m2l2 (01 +02) +m2lıl2 01cos92—mzlılz 010 2sinf 
dt (30, 


因此 ，2R 机 械 手 的 运动 方程 为 





da(| 32L IL zs sx ss sx is 
a=) T = (mi +#m2)L? 01 +m214 (01+ 02)+mzlıl2 (201 +62) cosb2 一 
a0 | 201 
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nial 128 «C261 ae Vor eee +m2)gli cost; 十 m2 gl2cos(01 +02) (11-340) 











d [IL IL Ss as is zoa 
e=] 50 m2l3(0, +02) +melil201cosO2 —m211128102sinbs 十 
a 2 © 2 


m2l1l201 (O102)sings +m2gl2cos(01 +62) (11-341) 
广义 力 Qi 和 Qs 是 驱动 广义 坐标 所 要 求 的 力 。 在 这 种 情况 下 ，Q1 是 在 基体 电动 机 处 的 
HET, Qo EE mi 处 电动 机 的 转移。 
运动 方程 可 被 调整 成 一 个 更 加 规则 的 形式 ， 即 


Qı =| (mı +m2)1? +moel2 (l2 Sop cosð2)]ő 十 m2l2(l2 if gusto o— 


2m2lilzsing20102 —m2lilssing202 + (mi +mz)glı cosh F#m2 gl2cos(01t0,) 























(11-342) 
Qz=mzl2 (l2 +lıcos02) 01 +m2l2 0z +məlılzsin020 ? +mzglzcos(6ı +02) 
(11-343) 
11.7 ARN Bi 
在 全 局 坐标 系 中 ， 刚 体 的 移动 和 旋转 运动 方程 为 
oF _ do (11-344) 
~ dt P 
“du 
°M = L (11-345) 
dt 
这 里 ， F 和 “M 表 示 作 用 在 刚体 上 并 且 在 质点 C 处 所 测量 的 合 外力 和 合 转 矩 。 矢 量 *p 是 动 
量 , 江 是 刚体 在 质点 C 的 动量 矩 。 
p=mv (11-346) 
L =rc Xp (11-347) 
在 刚体 局 部 坐标 系 中 ， 运 动 方程 为 
"p=" pHi ox" p =m" atm? aX” m (11-348) 
EM =" L +2 mX" Lam? ati os XCI ay) (11-349) 
这 里 , 工 是 刚体 惯性 矩阵 。 
了 Izy EA 
I=| Ix Iy le (11-350) 
Toz Izy Fz 
I 中 的 元 素 是 刚体 质量 分 布 的 函数 ， 定 义 为 
s=| o EÒ mn Dime jn ) dm i,j 二 1 ,2,3 (11-351) 


RE, Sj EY ASE O 函数 。 
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每 个 刚体 都 有 一 个 主 刚体 坐标 系 ， 在 该 坐标 系 中 其 惯性 矩阵 为 





I, 0 0 
Br=|0 I 0 (11-352) 
0 0 B 


在 主 坐标 系 中 ， 旋 转运 动 方程 可 简化 为 


M,=1,01—(U2—13) 203 
M2=I2w2—(13—1)1)w301 (11-353) 
M3=1303—(1,—I2) 102 


利用 章 次 位 置 矢 量 ， 我 们 也 可 以 定义 应 用 于 机 器 人 动力 学 中 的 伪 惯 性 矩阵 


















































Tee tl yy Rl x 
Dog Lg MXC 
2 
I Tax —Tyy eee I 
Ta Fr dm = 2 VEN 
. : ; Tax + Ly, 
ZX zy 2 MZC 
MXC myc mzc m 
(11-354) 
这 里 , rc 是 质心 在 刚体 坐标 系 中 的 位 置 。 
-| xdm 
mJB 
XC 
B 1f 
ro=|yc |= al ydm (11-355) 
mJB 
ZC . 
二 | m 
mJp 
对 于 具有 个 自由 度 的 机 械 系 统 来 说 ， 利 用 拉 格 朗 日 方程 可 求 得 其 运动 方程 。 
df 24) 3g 
. Q, r 1 ,2，…72 (11-356) 
dt 9 dq, 
q 
4=K -V (11-357) 
这 里 ，Y 是 系统 的 拉 格 朗 日 函数 ，K 是 动能 ，V 是 势能 ，Q, 是 非 潜 在 广义 力 。 
< Ifi Igi 4) 
: 了 ge (11-358) 
Q-= > (a5 Fo a OE ge, 


参数 g, r51, 2, 0, n) 是 系统 的 广义 坐标 ，Qi = Qi Qiy Qr)! 是 作用 于 系统 中 
第 ; 个 微粒 上 的 外 力 ，Q, 是 与 g 相关 联 的 广义 力 。 当 Cris yi zi) 是 微粒 m; 在 全 局 固 
定 坐标 中 的 稍 卡 儿 坐 标 时 ， 那 么 它 的 坐标 可 以 是 另 一 组 坐标 gi. qo. qas tty an 和 可 能 
时 间 z 的 函数 。 





Xi =f igi sd25G3 0% Gn st) (11-359) 
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Sis min 4 





Vi = gi (G12 G30 not) (11-360) 
zi =h; (qisg25d3-% Gn ot) (11-361) 
习题 
11-1 标记 与 符号 。 
描述 下 列 这 些 符号 的 含义 。 
Dopp DL 32°F DW ÐK ƏV 
7) dm 81 9 I; 10F 1D°M 12) 7 
13)0 @ 1428: 15)°Ag 16)°V, IDJ 18) X 


11-2 刚性 连 杆 的 动能 。 

















将 一 个 直 且 均匀 的 杆 作 为 机 械 手 的 刚性 连 杆 考虑 。 该 连 杆 的 质量 为 mx。 证 明 该 连 杆 的 
运动 可 以 表示 为 
K=- m: vi 十 vi。 v2 Fv * v2) 
XE, vi M v: 是 连 杆 端点 的 速度 矢量 。 
11-3 离散 的 微粒 。 
在 下 列 矢 量 处 分 别 有 3 个 微粒 mi 二 lkg、m2z 一 2kg、m3 一 3kg 
1 =] 2 
r= 1 72 一 3] nm=|—l 
1 2 一 3 
其 速度 为 
2 =] 3 
vi =|1 v2=| 0 v3 二 | 一 2 
1 2 = 


求 系统 在 质心 C 处 的 位 置 和 速度 ， 计 算 系 统 的 动量 和 动量 和 矩 ， 计 算 系 统 的 动能 ， 确 定 





动能 的 旋转 部 分 和 移动 部 分 。 
11-4 WA Cath) 坐标 系 中 的 牛顿 运动 方程 。 


证 明 : 刚体 (局 部 ) 坐标 系 中 牛顿 运动 方程 为 


Fz 


ax 0 We 
Fy |=mlay|+| wz 0 
F: Az “wy Wer 
11-5 ”曲线 路 径 上 的 功 。 


Wy ane 
一 oz || vy 
0 Us 


一 个 质量 为 m 的 微粒 在 一 个 圆 形 路 径 上 移动 ， 该 路 径 矢 量 为 
Srp 一 cosOf 十 sin0j +4K 
当 微粒 从 0 二 0 移动 到 0=r/2 时 ， 计 算 力 “F 所 做 的 功 。 


1) 


2—n2 PE 
i ee 


aay? ， 





G 
F atar (awe 


adar" 
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2) 


z2 一 y2 。 


a a a ae 





°F 


11-6 牛顿 运动 方程 。 


aa ag Gee 


基于 牛顿 法 ， 求 得 图 11-10 所 示 的 系统 运动 方程 。 





11-7 刚体 坐标 系 中 的 加 速度 。 


如 图 11-3 所 示 的 由 两 根 连 杆 构 成 的 机 械 手 ， 试 求 机 械 手 端点 表述 在 坐标 系 A 中 的 加 速度 矢量 。 


11-8 HE. 





XF FAEERE, oR AS EMERE AJY TY 





1) 


2) 


3) 








100 204/3 


D 


I=|20/3 60 0 


11-9 TRE RE. 





0 0 10 


对 于 位 于 质点 C 处 主 稍 卡 儿 坐 标 系 中 的 下 列 物体 ， 计 算 其 惯性 矩 。 

D 类 似 于 具有 圆 形 截面 的 均匀 臂 的 圆柱 ， 如 图 11-14 所 示 。 

2) 类 似 于 具有 长 方形 横 截 面 的 均匀 臂 的 长 方 体 ， 如 图 11-15 Bras. 

3) 类 似 于 具有 非 对 称 多 边 形 横 截 面 的 棱柱 杆 的 实体 屋 ， 如 图 11-16 所 示 。 























图 11-14 圆柱 





11-10 ”惯性 和 矩阵 的 旋转 矩阵 。 
惯性 矩阵 蔚 工 的 主 矩 阵 为 
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图 11-15 长 方 体 图 11-16 实体 屋 





sns zaan 








初始 的 刚体 坐标 系 绕 着 x 轴 旋 转 30" 接 着 再 绕 着 > 轴 旋 转 45" 后 ,， 便 可 得 到 主 坐 标 系 。 
计算 初始 的 惯性 和 矩阵 包工。 

11-11 惯性 矩阵 的 旋转 矩阵 。 

试 求 将 惯性 和 矩阵 工 变换 成 对 角 线 矩阵 所 要 求 旋转 矩 矩阵 。 








3 2 2 
I=|2 2 0.1 
2 0.1 4 








11-12 fA HERE 

对 于 位 于 质点 C 处 主 笛 卡 儿 坐 标 系 中 的 下 列 物体 ， 计 算 其 伪 惯 性 矩阵 。 
D 具有 长 方形 横 截 面 的 均匀 臂 ， 如 图 11-17 所 示 。 

2) 用 于 腕 关节 的 屋 形 的 复合 形状 ， 如 图 11-18 所 示 。 

X3) 由 3D 几何 形状 构成 的 复合 形状 的 夹 持 器 如 图 11-19 所 示 。 








a 

















图 11-17 等 截面 均匀 辟 图 11-18 用 于 腕 关节 的 屋 形 图 11-19 复合 形状 夹 持 器 


11-13 三 次 方程 。 
以 对 称 方式 求解 三 方程 
ax? +ba*+cx+d=0(a40) 
用 判别 式 4p? 十 g2 将 方程 转换 成 新 的 形式 。 
yi t3pytq=0 








AMER =y- J 


PE 268 —9abe+27a*d 





9a2 4 27a 
这 时 ， 解 是 

yı =Va —4B 

yeze" ae Ip 

ys=e Jae" 
这 里 








,VET p IHV FA 
2 2 
对 于 p, q 的 实数 值 : WMRAIREÆEEW, BWA DREKA, PPAR ESE EM; 
如 果 判 别 式 是 零 ， 那 么 有 3 个 实 根 ， 其 中 至 少 两 个 根 是 相等 的 ; 如果 判别 式 是 负 的 ， 这 时 有 
3 个 不 等 的 实 根 。 
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对 于 算 阵 的 特征 方程 应 用 这 个 理论 ， 证 明 主 惯性 矩 是 实数 。 

11-14 地面 上 运动 汽车 运动 学 。 

地 面 上 汽车 的 位 置 可 由 其 距 固定 子午 线 的 经 度 ， 即 格林 尼 治 (Greenwich) 子午 线 ， 和 
距 赤 道 纬度 9 描述 ， 如 图 11-20 所 示 。 我 们 将 一 个 具有 x 轴 并 且 位 于 地 心 处 的 坐标 系 B 附加 
到 附加 到 赤道 平面 上 ， 同 时 添加 一 个 指向 汽车 的 y FH, CLA SAE EMG, HOPE 
在 地 面 上 建立 的 坐标 系 ，G 是 全 局 坐标 系 〈 即 绝对 坐标 系 )。 证 明 : 坐标 系 B 的 角速度 和 汽 






































车 速度 为 
Bo, =Oint (weto)sindj p+ (we +) cosbk 
tup =—r (wg +o)coshig +rô k 
K 11-20 HAE o 和 纬度 0 所 定义 的 地 面 位 置 
计算 汽车 加 速度 。 


11-15 角 动 量 的 全 局 微分 。 
将 惯性 和 矩阵 了 I 和 角速度 Ewop 转 换 到 全 局 坐标 系 中 ， 然 后 求 取 角 动量 的 导数 。 用 另 一 种 
方法 证 明 : 








G qB G 


d è : 
a = To Wa) +E a,x? L=Ioa+@ XU) 


11-16 ”旋转 臂 的 运动 方程 。 

基于 拉 格 朗 日 法 ， 对 于 如 图 11-21 所 示 的 旋转 连 杆 试 求 其 运动 方程 。 

11-17 拉 格 朗 日 法 和 非 线 性 振动 系统 。 

利用 拉 格 朗 日 法 求 得 如 图 11-22 所 示 的 连 杆 运动 方程 。 线 性 弹簧 刚度 是 &。 

11-18 FARA ASIA He By 

11-23 所 示 为 一 个 具有 长 度 ! 和 质量 mx 摆 锤 的 单 摆 。 试 求 在 下 列 情 况 下 的 单 摆 的 运动 
方程 。 

D 支点 O EX 方向 有 一 个 受 迫 运动 。 
Xo =asinwt 


2) 支点 O 在 工 方向 有 一 个 受 迫 运动 。 
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Yo =bsinwt 


3) 支点 O 在 一 个 同上 有 一 个 受 迫 运动 。 





图 11-21 旋转 连 杆 























Y 
4 
-xy Va 
J 本 
0 
图 11-22 端点 处 具有 一 线性 弹簧 的 复合 单 摆 图 11-23 带 有 振动 支点 的 单 摆 


ro =Rcoswti+RsinotJ 
11-19 拉 格 朗 日 运动 方程 。 
考虑 一 个 拉 格 明日 物理 系统 








1 : : 1 
La sma stb y)? zka zby)? 


XE, Ritm., k, a 和 2 是 常数 。 

11-20 来 自 运 动 方程 的 拉 格 朗 日 运动 方程 。 
试 求 基于 下 列 运 动 方程 的 拉 格 朗 日 运动 方程 。 
1) 


mr? 0 | kılı0 | kol20 | mgl=0 





2) 
4g 


435 


As: 运动 学 、 动 力学 与 控制 技术 





r26 +2r70=0 

11-21 旋转 环 上 的 质量 。 

如 图 11-24 所 示 ， 质 量 的 微粒 自由 地 在 旋转 
的 垂直 环 上 滑动 。 该 圆 环 以 定常 角速度 w=p 旋 
转 ， 确 定 该 微粒 的 运动 方程 。 建 立 一 个 局 部 坐标 
A. HEr 轴 指 向 微粒 ，z 轴 位 于 圆 环 面 内 且 平 行 
于 质量 位 置 处 与 圆 环 的 切面 。 

Weill-22 ”电磁场 中 的 微粒 。 

证 明 质 量 m 的 微粒 基于 拉 格 朗 日 函数 为 





















































or cg 二 er .A 
其 运动 方程 为 
ag 3AN, e 全 à 
mqi e( aq: PF ) T rau Iq; Iq; 
这 里 


qı x 
q =]|q2 g 
q3 F 
这 时 ， 将 运动 方程 转换 成 一 个 矢量 形式 


mr =e E (r,—t)ter XB (r,t) 
XE, EMB 是 电场 和 磁场 。 





JA 


E =—Vọ Jt 


B =V XA 
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第 12 章 


机 器 人 动力 学 


由 牛顿 - 欧 拉 法 和 拉 格 朗 日 法 可 以 求 得 机 器 人 运动 的 动力 学 方程 。 牛 顿 - 欧 拉 法 是 比较 重 
要 的 ， 巾 该 方法 求 得 的 动力 学 方程 可 以 确定 使 机 器 人 移动 所 要 求 的 执行 器 作用 力 、 转 矩 及 关 
节 力 。 拉 格 朗 日 法 仅仅 提供 能 够 确定 执行 器 作用 力 和 转 矩 所 要 求 的 微分 方程 。 





12.1 刚性 连 杆 的 牛顿 - 欧 拉动 力学 





12-1 所 示 为 机 械 手 的 一 个 连 杆 及 其 速度 和 加 速度 矢量 的 特性 。 图 12-2 所 示 为 连 杆 
G) 的 自由 体 受 力图 。 力 F;-1 和 力矩 M;-1 是 在 关节 i 处 连 杆 (i 一 1) 作用 于 连 杆 (i) WA 
力 和 合力 矩 。 类 似 地 ，F; 和 M ;是 在 关节 i 十 1 处 连 杆 (i) 作用 于 连 杆 GHD 的 合力 和 合 
力矩 。 分 别 在 坐标 系 Bi 和 坐标 系 B: 的 原点 处 测量 并 标示 力学 系统 F, Mi) 和 
(F;，M;) 。 作 用 于 连 杆 OO 上 的 外 部 载荷 之 和 可 用 Fi 和 M6 表示。 




















图 12-1 Æ G) 及 其 矢量 运动 特性 


在 全 局 坐标 系 中 ， 连 杆 (i) 的 运动 牛顿 - 欧 拉 方程 为 
F,,—°F,+>°F.;=m;°a; (12-1) 
°M;_,—° M; +° Ma +Cd; ° r;)X? F; Cd; —°r;)X F; =° Ipa; (12-2) 
证 明 : 
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图 12-2 连 杆 G) 上 的 力学 系统 


在 连 杆 (i) 远 端 处 ， 力 学 系统 由 在 坐标 系 B; 原点 处 所 测 得 的 力 F; 和 力矩 M; 构 成 。 力 
F; 和 力矩 Mi 的 右 下 角 脚 标 是 一 个 用 来 说 明 坐 标 系 B; 的 数字 。 

在 关节 i 十 1 处 ， 总 是 有 一 个 使 连 杆 〈z) 作用 于 连 杆 G+) 的 作用 力 F;， 以 及 将 连 杆 
(i 十 1) 作用 于 连 杆 (i) 的 反作用 力 一 F;。 因 此 ， 连 杆 G) 上 总 有 一 个 来 自 连 杆 (i 一 1) 
的 作用 力 F;-1， 以 及 来 自 连 杆 GHD 的 反作用 力 一 F;。 作 用 力 被 称 为 主动 力 (driving 
force), 及 作用 力 被 称 为 从 动力 (driven force), 

类 似 地 ， 在 关节 i 十 1 处 总 有 一 个 使 连 杆 (i) 作用 于 连 杆 GHD 的 作用 力矩 M;， 以 
及 将 连 杆 (i 十 1) 作用 于 连 杆 (i) 的 反作用 力矩 一 M;。 因 此 ， 在 连 杆 (i) 上 总 是 有 一 个 
来 自 连 杆 (i 一 1) 的 作用 力矩 M;-1， 以 及 来 自 连 杆 GHD 的 反作用 力矩 一 M;。 作 用 力矩 
被 称 为 主动 力矩 (driving moment)， 反 作用 力矩 被 称 为 从 动力 和 矩 (driven moment). 

因此 ， 在 坐标 系 B;-1 的 原点 处 有 一 个 驱动 力 系 统 (F;-1，M;-1)， 在 坐标 系 B: 原点 处 
有 一 个 从 动力 系统 (F;，M;)。 主 动力 系统 (F;-1，M;-1) 使 连 杆 G) 产生 运动 ， 从 动 
HRE (F. Mi) WEI (i 十 1) 产生 运动 。 

除了 作用 力 和 反作用 力 系统 之 外 ， 也 可 能 有 一 些 作 用 于 连 杆 Gl) 的 外 部 力 ， 在 质心 Ci 
处 可 形成 合成 力 系统 (2F;，> M6)。 在 机 器 人 应 用 中 ， 重力 通 常 是 在 中 间 连 杆 上 的 唯一 
外 部 载荷 ， 来 自 环境 的 反作用 力 是 基体 连 杆 和 末端 执行 器 连 杆 的 附加 外 部 载 苟 。 基 体 执行 器 
作用 于 第 1 个 连 杆 的 力 和 力矩 是 Fo。 和 Mo， 末端 执 行 器 作用 于 环境 的 力 和 力矩 是 本 ,和 RM，。 
如 果 重 力 是 作用 于 连 杆 (Gi) 的 唯一 外 部 载荷 ， 并 且 它 位 于 一 "k, 方向 上 ， 则 有 

DF,=m;'"g =—mie'k, (12-3) 
D? M 一"r Xmi'g 一 一 "Pr Xmigrko (12-4) 
这 里 ，g 是 重力 加 速度 矢量 。 

如 图 12-2 所 示 ， 我 们 用 于 "mr; 标示 连 杆 质心 的 全 局 位 置 撩 量 ,， 分别 用 'd; 和 人 ;标示 刚体 
坐标 Be 和 B;-1 原 点 的 全 局 位 置 。 在 质心 C; 处 ， 可 以 测量 和 显示 连 杆 的 速度 v o0; 和 加 速 
BEd; . @;. EF G) 绕 着 其 质心 Ci 的 质量 m; AREE. I, 可 以 确定 其 物理 特性 。 

由 牛顿 运动 方程 可 知 : 在 质心 C; 处 ， 施 加 于 连 杆 (i) 的 合力 等 于 连 杆 质量 乘 以 其 加 
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速度 。 
F; ,—°F,+ °F, =m:°a; (12-5) 
对 于 欧 拉 方 程 ， 除 了 作用 力矩 和 反作用 力矩 ， 还 必须 加 上 绕 着 质心 C; 作用 力 和 反作用 
力 的 力矩 。 力 一 下 和 一 下 -1 的 力矩 分 别 为 一 mi; X F: Mn: X F;-1. Pm 是 点 oj 距 质心 C; 
的 位 置 撩 量 ，n; 是 点 o;-1 距 质心 C 的 位 置 矢 量 。 因 此 ， 连 杆 的 欧 拉 运动 方程 为 : 
Mi —° M; + X° Ma nn X’ F; —° m; X’ F; =° Loa; (12-6) 
然而 ， pe 








oni =d; | 一 "rr， (12-7) 
om; =" d;—°r ; (12-8) 
7-14; =° m;—"n; (12-9) 


以 便 导 出 式 (12-2), 
因为 机 器 人 的 每 根 连 杆 都 有 一 个 平 动 运动 方程 和 一 个 转动 运动 方程 ， 因 此 对 于 7 根 连 
杆 的 机 器 人 人， 有 2n 个 矢量 运动 方程 。 然 而 ， 
由 于 涉及 2(n 十 1) 个 力 和 力矩 ， 因 此 必须 确 
定 一 组 力 系 统 (通常 F, 和 M,)， 以 便 求解 方 
程 获得 关节 力 和 力矩 。 
例 12-1 单 根 连 杆 机 械 手 。 
图 12-3 所 示 为 一 根 连 杆 ， 该 连 杆 在 原点 
O 处 通过 一 个 球形 关节 系 连接 至 地 面 。 连 杆 
自由 体 受 力 由 在 末端 点 处 的 一 个 外 力 和 转 矩 、 
重力 及 在 关节 处 的 驱动 力 和 驱动 转 矩 所 构成 。 
连 杆 的 牛顿 - 欧 拉 方程 为 
OF +°F.+mgK =m°ac (12-10) 
°M,+°M.+°n X° Fo +°m X oF, = Lo 
(12-11) 
为 了 理解 其 应 用 ， 现 考虑 图 12-4a 所 示 的 均匀 梁 。 图 12-4b 所 示 为 梁 的 受 力图 及 其 相关 
的 位 置 矢量 m 和 n 。 


























F, 


2 

















图 12-3 单 根 连 杆 机 械 手 











l 
Fos = cosh 
°m =| L. n= L. (12-12) 
z sind z sind 
0 0 
梁 的 运动 学 关系 为 
“一 一 "11 (12-13) 
全 一 一 有 十 m, (12-14) 
ie Hr, 表示 在 坐标 系 Bu 中 质心 CME, di 表示 在 坐标 系 Bo 中 末端 点 的 位 置 。 
@, = OK (12-15) 
9 @ =001=0 K (12-16) 
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a) b) 
图 12-4 ”旋转 的 均匀 梁 


le l. 
— ; 2 
2 0 sinf + 9 0? cosh 











ac= 0 X’ roo XG Xr )= 了 (12-18) 
2 2 
0 
作用 于 梁 上 的 力 有 : 
Fx 0 
五 ,一 | Fy "下 .一 |0 (12-19) 
Fz 0 
Qx 0 
'M,=|Qy | °M.=|0 (12-20) 
Qz 0 
让 我 们 假设 梁 绕 其 质心 的 质量 矩阵 :Ti 为 
Ls 0 0 
n= I, 0 (12-21) 
0 I 
cosO —sind 0 
OR; =Rz =| sind cos) 0 (12-22) 
0 0 1 
I, 0 0 T,cos*O+T,sin?@ (I,—I,) cosgsin0 0 
°l =Rz'I R} Ri 0 I, 0|°RI= (I,—I,) cosOsin0 Tecos20 二 Tsin20 0 
0 0 T, 0 0 I, 
(12-23) 
取代 式 (12-10) 和 式 (12-41) 中 的 上 述 信息 后 ， 可 得 下 列 运动 方程 。 
F, HF. Hmi g =m’ ac (12-24) 


440 


第 12 章 “机 器 人 动力 学 “全 





1 : . 
——mil(0 sin0 一 02cosO) 


Fx 2 
F =| 1 . : (12-25 
7 —m,1(@ cosd t0? sind) +mig l 
Fz 2 
0 
°M, +° M.+°n X° Fot rmX°F.=Io ec (12-26) 
L 
z F zsinð 
Qx i 
Qy |= -Fz COSO (12-27) 
Qz 


1.6 +5F ycos— $F x sind 
现在 取代 式 (12-25) 中 的 力 分 量 ， 以 确定 驱动 力矩 "Mo 的 分 量 。 
0 


Qr |= : (12-28) 


Qz (1.47) é + Sm glcosð 
例 12-2 ”四 杆 机 构 动 力学 。 


Qx 











m, § 
Mo M 


-Mo 





-Fo 


b) 
图 12-5 四 杆 机 构 及 每 根 连 杆 的 自由 体 受 力图 


12-5a 所 示 为 一 个 闭环 四 杆 机 构 。 每 根 连 杆 的 自由 体 受 力图 如 图 12-5b 所 示 。 因 为 给 
出 了 质心 位 置 ， 因 此 每 根 连 杆 的 矢量 %; 和"m; 也 是 已 知 的 。 连 杆 (i) 的 牛顿 - 欧 拉 方 程 为 
F, —°F, +migh =m;°a; (12-29) 
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IOM;_ —° M; +°n; X? F; —°m; X’ F; =1; 0a; (12-30) 
因此 ， 有 三 组 方程 : 
°F, —°F, +migl=m,°a, (12-31) 
°M, —° M,+°n, X° Fy —° m, X°F, =I o0; (12-32) 
°F, — F; +m:gf =m2°a, (12-33) 
°M, 一 " M, +°n: X° F, —° m, X’ F, =I 2,0 (12-34) 
F, 一 "FF +msg] =m2°a, (12-35) 
° M, —° M; +n; X°F,—° m; X’ F; =13 03 (12-36) 
假设 关节 处 没有 摩擦 ， 机 械 机 构 是 平面 机 构 ， 则 力 矢 量 位 于 XY FEN., JEFF Z 
轴 。 因 此 ， 运 动 方程 可 简化 为 
°F, —°F, +migl =m: a] (12-37) 
M+ nn X°F)—° m X°F, =I, 0, (12-38) 
F, —° F, +mzg] =m: , (12-39) 
nX? F, —° mX? F; =I20, (12-40) 
0 再 ,一 "下 ;十 msg =m: , (12-41) 
°n,X°F,—°m,X°F3=13,@; (12-42) 
这 里 ,"M, 是 机 械 机 构 的 驱动 转 和 矩 。 方 程 数 可 减少 至 9 个 方程 ， 机 械 机 构 的 未 知 数 是 : 
Fox Foy .Fie.Fiy Far .F2y .F 32 .F3y Mo (12-43) 
以 矩阵 形式 重新 排列 这 组 方程 ， 有 
Ax =b (12-44) 
其 中 ， 1 0 —l 0 0 0 0 0 0 
0 1 0 = lb 0 0 0 0 0 
Niy My May =M], 0 0 0 0 1 
0 0 1 0 =] 0 0 0 0 
A= 0 0 0 1 0 一 站 0 0 0 (12-45) 
0 0 Noy No, Mo, Mo, 0 0 0 
0 0 0 0 1 0 = 0 0 
0 0 0 0 0 1 0 al 0 
0 0 0 0 一 103y Ng, msy TMz, 0 
Fox Maly 
Fo Maiy TMg 
Fiz liay 
Fiy 70.2Q 27 
x =| F2: | b =|ma2, = Mmg (12-46) 
Foy Izas 
Faz M303, 
Fsy M343, M3g 
Mo Iza; 
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JERE A 描述 了 机 械 机 构 的 几何 参数 ， 矢 量 x 是 未 知 力 ， 矢 量 表示 动态 项 。 为 了 求解 四 
杆 机 构 的 动力 学 ， 我 们 必须 计算 加 速度 "ae; 和 ow,;， 求 取 所 要 求 的 驱动 转 矩 M 。 和 关节 力 。 
F.=F;—F> (12-47) 
力 正 、 被 称 为 振动 力 ， 表 明 机 构 对 地 面 的 反作用 力 。 
例 12-3 带 有 末梢 质量 的 旋转 均匀 梁 。 
考虑 在 末梢 点 处 带 有 及 挂 质量 的 均匀 梁 ， 如 图 12-6a 所 示 。 图 12-6b 所 示 为 梁 的 自由 体 
受 力图 。 梁 的 质心 位 于 'r 1 处 。 






































mı +2m2 
b/2 L or I rg 
P” / n i E 2 (mı +m2) E aa 
ri mi 二 mo mi 十 m2 9 0 | 
0 
0 
其 相关 的 位 置 矢量 m Mn 为 

lni =l r=- ri (12-49) 
lm; =i! ry =U-rezdi (12-50) 
°d=—'! n+! m =li. (12-51) 

(一 mr )cosO —r „cosl 
°m =| (1 一 rz)sin0 | °n=| —r,zsind (12-52) 

0 0 
Fy 
2, 
mg 
ms 
b) 
图 12-6 末梢 点 处 悬挂 着 重 物 m WIR 
梁 的 运动 学 为 
0 1=0K (12-53) 
9&1 =, @,=0K (12-54) 
poe (12-55) 
Tr 6 sind +r, 62 cos 

aes a@,X°ri ty) @, XQ @,X°r)= r,6 cosOt+r, 62 sind (12-56) 


0 
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BEN AA 
Fx 0 
°F =|Fy | °F.=j0 (12-57) 
Fz 0 
Qx 0 
°M =|Qy | °M.=|0 (12-58) 
Qz 0 
假设 2 是 梁 绕 质心 的 质量 矩 矩 阵 
I, 0 0 
'n=/0 I, 0 (12-59) 
0 0 “LT. 
这 时 ， 机 械 手 在 矢量 rı 处 机 械 手 绕 公共 质心 的 质量 矩 矩阵 为 
I, 0 0 
'y,=|0 I, 0 (12-60) 
0 0 Ts 
gy2 
b= +Mi(r, z) FM: (>r)? (12-61) 
已 知 变换 矩阵 "RI ， 我 们 可 以 确定 "了 。 
cosO 一 Sin0 0 
"及 一 Rzo 一 | sind cosd 0 (12-62) 
0 0 1 
I, 0 0 T,cos*O+Ty,sin?@ (I,—I,)cosdsind 0 
°F, =Rz w IR} ¿=°R;,| 0 I, 0 | °R?=|(C12—I,)cos@sin? I,sin?@+I,cos?é@ 0 
0 O ds 0 0 了 3 
(12-63) 
取代 式 〈12-10) 和 式 〈12-11) 中 的 上 述 信息 ， 可 以 求 得 下 列 运 动 方程 : 
°F + F. tmigK =m? ac+m? i (12-64) 
Fx —(m,+mz2)r; (0 sind—62cos0) 
Fy |= (mi +m2)rz(6 cosd+62 sind) +(m;+m2)¢ (12-65) 
Fz 0 
M+ Mtn Xo Fy tom Xo F =I (12-66) 
Qx rF zsinð 
Qr |= = rF z cos (12-67) 


Qz 136 +r, Fycosô—r,F x sind 
Fast (12-65) 中 的 力 分 量 取代 式 (12-67) 中 的 相关 信息 ， 则 可 得 驱动 转 矩 "M ,的 





Sve meine Gi 








Qx 0 
Qy |= 0 (12-68) 
Qz [Ts 十 Ci +m2)r2. 6 +m +m) rig cosh 
取代 ras MoE A FGF 0. 
Qo=07= [1 + (Tm +m: Jre |ë + (Bm +m: Jgtcoso (12-69) 


例 12-4 2R 平面 机 械 手 牛顿 - 欧 拉 动力 学 。 








2R 平面 机 械 手 及 其 自由 体 受 力 情 况 如 图 12-7 所 示 。 驱 动 器 的 转 矩 平行 于 Z Hh. Hoo 














和 Q 1 表示 。 第 1 个 连 杆 的 运动 牛顿 - 欧 拉 方程 为 


"Fu 一 "FI 十 18 =m)°a, (12-70) 
°Qy)—°Q, Hn X° Fy — m XO Fy =° Ioa (12-71) 





























mg 
图 12-7 2R 平面 机 械 手 的 自由 体 受 力图 
第 2 根 连 杆 的 运动 方程 为 g 
°F, +m g] =m2°a> (12-72) 
"Os hak F =I Be (12-73) 
对 于 4 个 未 知 变 量 F，。、F1、Q。 和 Q |;， 有 4 个 方程 。 这 些 方程 可 以 用 和 矩阵 的 形式 为 
A=b (12-74) 
这 里 
1 0 0 =] 0 0 
0 1 0 0 =i 0 
l (12-75) 
0 0 0 Í 0 0 
0 0 0 0 0 
0 0 0 No, No» 1 
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Fiz Mia] 
Foy Maly Mig 
0 
Qo liai 
x= b= (12-76) 
Fis 7722Q 27 
Fiy Mod2y M28 
Qi oT a 








例 12-5 ”关节 执行 器 方程 。 
在 机 器 人 动力 学 中 ， 我 们 不 需要 求 得 关节 力 。 执 行 器 转 矩 是 非常 重要 的 ， 因 为 转 矩 用 来 
控制 机 器 人 。 在 例 12-4 中 ， 我 们 确定 了 图 12-7 所 示 的 2R 平面 机 械 手 关节 力 系统 的 4 个 








方程 。 
°F —° Fi +migl=m,°a, (12-77) 
"Qo0—" Qt niX Fo m, X° Fi1="IT10@1 (12-78) 
°F +m:g] =m: a> (12-79) 
Q+’ nX F= Ioa > (12-80) 


然而 ， 可 以 消除 关节 力 正 和正 1;， 并 且 将 方程 数目 减少 到 含有 两 个 转 抢 变量 @Q ;和 @ ;的 
两 个 方程 。 消 除 式 (12-79) M (12-80) 之 间 的 Fi!1， 则 有 
90, ="Tag wy 一 "mv X Ome X a,—m2g)) (12-81) 
消除 式 (12-77) MÈ (12-80) 之 间 的 Fo。 和 F1， 则 有 
Q, = QH I oa it mi Xm X ay—mgh)— ni XmiX ay —mighl tm X°a2—mogs) 











(12-82) 

如 果 有 兴趣 ， 关 节 力 Fo MF 为 
°F 1=m2" a; —m:g] (12-83) 
°F 5m: a 1+Hm az — (mi tmz) gl (12-84) 








例 12-6 具有 重 载 机 械 辟 和 关节 的 2R 平面 机 械 手 。 

对 于 2R 平 面 机 械 手 来 说 ， 在 真实 的 环境 中 我 们 通常 在 关节 0 处 设置 一 个 大 功率 的 电动 
机 以 转动 连 杆 (1)， 在 关节 1 处 也 设置 一 个 大 功率 电动 机 以 转动 连 杆 (2)。 在 末梢 点 处 我 们 
也 可 以 用 夹 持 器 运载 重 物 。 关节 0 处 的 电动 机 一 直 位 于 地 面 ， 其 重力 并 不 影响 机 械 手 的 动力 
学 。 机 械 的 自由 体 受 力 类 似 于 图 12-8 中 的 自由 体 受 力 。 

重 载 关 节 将 取代 质心 Ci 的 位 置 ， 并 改变 相关 的 位 置 矢量 m 和 n 。 假 如 我 们 确定 如 图 
12-9 中 所 示 质 心 C; 新 位 置 的 矢量 m 和 n ， 我 们 将 得 到 与 式 (12-70) 一 式 (12-72) 相同 的 运 




































































动 方程 。 
0 机, 一 "下 ;十 (Oil 十 jz)g =m?’ a, (12-85) 
"OO nX’ FO mX’ Fe Ti es (12-86) 
°F +m +m) gf =(ma +m) a, (12-87) 
Oy” we Fy ="La? (12-88) 


我 们 可 以 将 质量 表示 为 
mı 二 711 十 77212 (12-89) 
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Mm 
128 
mis 


Al 12-8 具有 重 载 机 械 臂 和 关节 的 2R 平面 机 械 手 








(mtm ) & 





(mt ) 8 F, 


Q; 
图 12-9 确定 质心 C; 新 的 位 置 矢 量 m Flln 


m2 =m; +m22 (12-90) 
从 而 可 利用 相同 的 具有 非 对 称 质心 的 式 〈12-79) 一 式 (12-80)。 
例 12-7 2R 平面 机 械 手 的 一 般 方程 。 
让 我 们 分 析 一 个 具有 质量 的 机 械 臂 的 通用 2R 机 械 手 ， 该 机 械 手 承载 如 图 12-10 中 所 示 
的 有 效 载 荷 mo. 

















运动 方程 为 
°F, —°F,+mighf=mi°a, (12-91) 
"Oo Q +° nX Py m, XF; =I a: (12-92) 
°F, +m +m)g] =m2°a> (12-93) 
Q, +° nX F, +° m; Xmog] = Ine, (12-94) 
TARR AF o. Fi. WH FIA KEQ o PMO 1 的 方程 : 
Q =I —’ n X[m: a; — (mi tm.) gl l—°m,Xmogl (12-95) 
0 =° Q +° Ioa +H m Xn: "as —(mo+m)gj) 
—°n X[m: a; +m a, — (mo +m tm) gS] (12-96) 
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mg 
图 12-10 具有 质量 机 械 辟 并 承载 有 效 载荷 mo 的 2R 机 械 手 








j 


























力 Fo。 和 1 为 
oF =m2°a,—(mi tm gs (12-97) 
°F =m: a, +m? a; — (mo tm +mz)g J (12-98) 
1 一 一 "有 1 (12-99) 
or = Ri m,n, (12-100) 
"di=—" ni mi (12-101) 
id: =—°n: +° m, (12-102) 
°d,=—°n,+°m,—°n,+°m, (12-103) 
通常 情况 下 ， 质 心 C; 的 局 部 位 置 矢 量 为 
一 clcosO1 
m1 = Ri n; =—Rz,0¢1' =| 一 clsin0) (12-104) 
0 
—c2cos(@, +02) 
n= RR, n =—°R1' Ro? m2 =—Rz.9,Rz.0202" 12 =| 一 czsin(0 +02) 
0 
(12-105) 
—c2cos0z 
"ny =—'R;’ n =—Rz,9,€2" i2=| —c2 sind, (12-106) 
0 
(11—c1)cosO1 
°m =° R; mi=—Rz,o(li—ce)! =| (1 一 ci)sin0 (12-107) 
0 
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(l2—c2) cos 














°m ,= Rs m; =° R; (l2 —c2)? i2 =| U2 —c2) sine 
0 
这 里 
cosO1 一 Sin01 0 
°R, =Rz,9, =| sind cos0; 0 
0 0 1 
cos? 一 Sin0y 0 
I'R; =Rz o, =| sinf? cos0 0 
0 0 1 
cos(@; +02) sin(0; +02 
°R,=Rz,.0, +02) =| sin(O +02) cos(@; +02) 
0 0 
相关 位 置 矢 量 为 
cı cos 
or 1 一 一 "1 一 "RiIIri 一 "Ricl 一 | clsin0 


0 





A, +02) 
01 +02) (12-108) 
(12-109) 
(12-110) 
) 0 
0 (12-111) 
1 
1 
1 (12-112) 


lı cos +c2cos(@, +02) 
oF ,= Ri mi nn, = dR r=] lisindi +czsin(01 +02) | (12-113) 








lı cos 
^d, =—’° n, +° m= Lı sin@; 


0 


Zocos(@; +02) 
d2 =—?° nz: +° m=] l2sin(6; +42) 


0 





Z,cos@, 4 





0 


(12-114) 


(12-115) 


tL2cos(@, +02) 





0 0 0 0 0 0 = e 
"4 ,= ni mi ons" m, = d, +? d= Zi sin, 7 


连 杆 的 角速度 和 和 角 加 速度 为 : 

ug 1 =01K 

0 1 =) ©, =f, R 

00 2 =(6,+62)K 

ou 一 0 0, = (NO)K 
质心 Ci 的 平 动 加 速度 为 


Fl2sin(01 十 02) |(12-116) 
0 


(12-117) 
(12-118) 
(12-119) 
(12-120) 
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一 cl101sin01 十 cl 01cosO1 


0 


0 = 0 — si : 5 
a; =a] X fi 00: X QX rı) cı 0icosbl 十 cl 67 sind, (12-121) 


0 


— lı 6; sinfı +1, g? cos01 





OF — 0 — 0 = a è z 
dı =ù æ X d 1 o0 XC @, X d) lı 6,c0s0,; +1, 0? sind; (12 122) 
0 
G 420 Vd ar 
oy d'd, oF g 0 0 0 0 
4 一 一 一 一 do2Xxid im X( @:X]d2)5|07, (12-123) 
dt? 2y 
0 


od, =— l 61 sind; — l 02sin(01 +62) +1, 63 cos +1, 62 cos(O1 +62) (12-124) 
od, =l 61c0s01 +12 的 cos(0 +02) +11 6% sindi tle 02sin(g +02) (12-125) 


°a,=° ds yay XC rp —"d @,X[>@.XCOr,—°d,)] 


=dy—) @2X° met) 2X (9 2 X° m2) =| ar, (12-126) 





"ay, =C c2) (6, +62) 1262 ]sin(0ı +02) —1 Oe 十 1103 cos01 一 








Ca —c2) (6, +62)? —1262 1cos(01 +02) (12-127) 





"as => [U — c2) +65 )— Z202 Jcos(O1 +62) +116; cost +1163 sind; 一 











[U —e2) G1 +82)? — 1263 ]sin(@1 +62) (12-128) 
全 局 坐标 系 中 的 惯性 矩阵 为 

In 0 0 Talicos201 十 Tisin20 (Ta 一 Ta)cosOisin0l 0 

°T, =Rz.0, RS =R; | 0 In 0 | °RE=} ai —Iy1)cos0) sind Tuacos20 十 Tsin20 O 
0 0 Ia 0 0 Ia 
(12-129) 

I2 0 0 T,2co8’ O12 十 Tasin20 (12 一 Tvz)cosgizsin0iy 0 

"I, =R?’ RI =°R,| 0 Iy 0 | CRI =] (02 一 Ts)cosgizsingly Tvacos202 十 Tuzsin20ly 0 
0 0 Iz 0 0 了 -2 
(12-130) 
O12 =01 +02 (12-131) 


取代 式 (12-95) 和 式 (12-96) 中 的 这 些 结果 ， 求 解 2R 机 械 手 的 动力 学 方程 可 得 Qu 和 @ 1 。 


Oi =I æ — l n X[m: az — (mo tm) g J ]—°m;Xmog J= (12-132) 
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"O =U 22 +moc%3—melec2 +m2l1c2c0s62)6; + (I 29 +m203)62— 








m2116203 sind: — (macs tmols)g cos(01+02) (12-133) 
Qo =9Q, HI oa, +m; X[m a — motma)g 门 一 
0 
pm X{m,°a,—mig J +[m2°a,—(motmarg J ]}=| 0 (12-134) 
"Qos 





Qo = (la HI Hmc? +Hm[1 +e? — lc +l (2c2 —12) cos02 1}61 + 


[Iz Hmc (c2 +1icos02) 0 —m211120? sinb» +malica0? sing; 一 








2mol (12—c2)0102 sinz (72021 十 721c1 十 72221)gcosOl 一 
(m2c2+mol2)gcos(O, +02) (12-135) 
kH 12-8 ”运动 方程 的 矩阵 形式 。 
让 我 们 将 式 (12-133) MIÈ (12-135) 重新 调整 成 矩阵 形式 。 

















D(q)q +C(q .q)q +G (q)=@ (12-136) 
a oa (12-137) 
a 02 "Q:- 
Z1—Z2+Z3cosb2 Z\ 
D(q)= (12-138) 
Zi +Z 4 Z2 Z5 + Z 6 cOSO2 Z1 十 ZacosO> 
. — Z; fi sinb? 0 

C(q,9) 一 . . . : (12-139) 

(一 27 6,—Zs 02)sing» (Z3 02 一 28 01)sin0， 

—Z ,cos(@, +42) 
G (q)= (12-140) 
—Zocos(O; +02) +Zi0cos)1 

Zi =Il22+m2c3 (12-141) 
Zo=m212¢2 (12-142) 
Z3=moel\c2 (12-143) 
Za4=Iatmic3 (12-144) 
Zs=m2li (12-145) 
Zo=m,l1(2c2— /2) (12-146) 
7 一 71027 C2 (12-147) 
Zs8=m2l (12—c2) (12-148) 
Z9=(mo2c2+mol2)g (12-149) 
Zio =(moli tmicitmeli dg (12-150) 





KB 12-9 FHL 2R 机 械 手 的 关节 力 。 

取代 式 (12-97) MÈ (12-98) 中 有 关 式 (12-104) ~st (12-130) 的 矢量 信息 ， 我 们 求 
得 常规 2R 机 械 手 的 关节 力 如 图 12-10 所 示 。 机 械 手 的 机 械 臂 质心 在 C; 处 ， 并 且 承 载 有 效 载 
T m0。 
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‘Fiz 
°F =m2°a,—(motmag J = B (12-151) 


ere ae 202sin(O1 +02) + 
m2c202cos(01 +02) + ig 22 ens 00) eel cos 10? + 
2mol1 (l2 —c2)010zcos(91 +62) (12-152) 
° Fy, =L—m2 (12 —c2)cos(O1 +62) +m2l1 cosh, 16; Anges Os cos(CO1 +42) 十 





m2c202 ante | 02) | [ mo (ls» caJsintð 4-03) nal en ]02 一 
2m2l1 (12 —c2)61028in(61 +02) —(motm2)g (12-153) 
"Fas 
°F =m. a, +m? a; — (mo +m +mz)g = Fo, (12-154) 
0 








°F, =[me2(2—c2)sin(61 +62) —(mic1 +me2l1) sind: 10; —m2c262sin(; +02) — 








macz02cos(O1+0 H0 2) HE m2 (ls c2)cos(01 F02) | (mel) mre eee 1 


2m2 (la 一 cy 616200801 +62) (12-155) 





°F oy =L m2 (l2—c2)cos(O; +42) + (m2l1 tmic1)cos01 161 +m2c262c0s(61 +02) + 








phat 0 ia: HF-0;) 十 [一 mz (72 一 c2)sin(01 十 9s) 十 (m21l1 Fmici) sing1 ee 一 








2mzli(ls—c2)0102sin(01+02) — (mo +mı +mz)g (12-156) 


12.2 ”递归 牛顿 - 欧 拉动 力学 








在 机 器 人 应 用 中 ， 牛 顿 - 欧 拉 运 动 方 程 的 优点 是 在 某 一 时 刻 我 们 可 以 计算 连 杆 关 方 力 。 
因此 ， 从 末端 执行 器 连 杆 开始 ， 我 们 可 以 一 个 一 个 地 分 析 连 杆 ， 在 基体 连 杆 处 结束 ， 反 之 亦 
然 。 对 于 这 样 的 分 析 ， 我 们 需要 改造 牛顿 - 欧 拉 运动 方程 使 其 应 用 于 感 兴趣 的 连 杆 坐标 系 。 

ÉI (i) 在 其 刚体 坐标 系 B; 中 的 向 后 递归 牛顿 - 欧 拉 运动 方程 为 




















°F; = F; X F gn ae (12-157) 
'M,-,='M,—'M,,—(C d;-,—'r;) X' F; 

+Cd;—'r;)X'F;+'l,)a@;+)@;X'1;) 0; (12-158) 

= da Py (12-159) 

‘m;='d;—'r; (12-160) 


当 求 得 坐标 系 B 中 的 驱动 力 系 统 CF 1 M ;_1)， 我们 便 可 以 将 其 转换 至 坐标 系 
Bi;-1 之 中 ， 并 且 对 连 杆 (i 一 1) 建立 牛顿 - 欧 拉 方程 。 

a eee ee eae eee (12-161) 

“IM, = M4 (12-162) 
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对 于 连 杆 (i 一 1) 来 说 ， 负 的 转换 力 系统 表示 受 载 力 系统 (一 Fi Z M;i). 
ERP G) 在 其 刚体 坐标 系 B; 中 的 正 问 递归 牛顿 - 欧 拉 运动 方程 为 

















IP= F, HSF moa, (12-163) 
M: = M; a +X Mt Od ri X F; aS Cd; r) 

XIF, —I iaio, XI io, (12-164) 

i n;=id; I—ir,; (12-165) 

im, ='d;—'r, (12-166) 

当 求 得 坐标 系 B 中 的 反作用 力 系统 CF, ,M, )， 我 们 便 可 以 将 其 转换 至 坐标 系 Ba Zh 

Hip STOF, (12-167) 

HM =TM, (12-168) 


对 于 连 杆 (i 十 1) 来 说 ， 负 的 转换 力 系统 表示 作用 力 系统 (一 1F,;, —'*'M;). 
UE AA : 
在 刚体 坐标 系 中 ， 刚 性 连 杆 的 欧 拉 方 程 为 























Gq i , _ 
errs PL =" L + øX’ L ='I,;@;+6 @3X'1;; 0; (12-169) 
这 里 , L 是 连 杆 的 角 动 量 。 
BL 一 5T8 @, (12-170) 
对 于 施 力 系统 ， 我 们 可 以 求解 式 (12-1) MA (12-2) 的 牛顿 - 欧 拉 运动 方程 。 
"有 ;一 "下 ;一 之 "下 .十 ia (12-171) 
0-40 
Miam Me Meat da Bia Cd De Fi L 
(12-172) 
这 时 ， 将 方程 转换 至 附加 于 连 杆 〈z) 上 的 坐标 系 B ， 以 便 确定 牛顿 - 欧 拉 运动 方程 的 递归 形式 。 
F; =T; 0 F; = F; X Ba ie, Oe (12-173) 
0 
; _ : ; , : ; ; ; l d 
Mi 一: 一 下: M; =M; -X M.-C d; r;)X' Ftd; r) X Fitt 
(12-174) 
'M,-,=°T ; VM; a5 M; -X M; Cd; 1 一 ri) 


XF; at Cd; —'r;)X F; +L @ +e; XI bo; (12-175) 
从 连 杆 (i) 开始 求 得 前 一 个 连 杆 《一 1) 的 运动 方程 ， 该 运动 方程 被 称 为 逆向 牛顿 - 欧 
拉 运 动 方程 。 
我 们 也 可 以 从 连 杆 (i) 开始 求 得 下 一 个 连 杆 (i 十 1) 的 运动 方程 。 这 种 方法 被 称 为 正 
向 牛顿 - 欧 拉 运动 方程 。 利 用 式 (12-157) M (12-158) 的 牛顿 - 欧 拉 运 动 方 程 ， 我 们 可 以 
在 附加 于 连 杆 G 上 的 坐标 系 B; 中 写 出 其 正 向 递归 形式 。 




















Fo Bey uF gm: (12-176) 

MT ;一 RM WO 
OF 11; oa: —0@ ; X11; 00: (12-177) 
4 (12-178) 
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im, ='d;—'r; (12-179) 
利用 式 (12-176) 和 式 (12-177) 中 的 正 向 牛顿 - 欧 拉 运动 方程 ， 我 们 可 以 通过 施 力 系统 
CF: M:a) 计算 反作用 力 系统 CF;,'M ;)。 当 在 坐标 系 B: 中 确定 反作用 力 系 统 
('F;,'M,;) 时 ， 我 们 可 以 将 其 转换 至 坐标 系 Baz. 
iF =" FF; (12-180) 
1M = "To Me (12-181) 
对 于 连 杆 GHD 来 说 ， 负 的 动力 转换 系统 就 是 施 力 系统 ， 我 们 可 以 将 牛顿 - 欧 拉 方 程 应 用 
于 连 杆 (i 十 1)。 

正 向 牛顿 - 欧 拉 运动 方程 允许 我 们 从 一 个 已 知 的 施 力 系 统 QF。,*M。) 开始 ， 该 施 力 系 
统 是 从 基体 连 杆 作用 于 连 杆 CL) 的 作用 力 ， 计 算 下 一 个 连 杆 的 作用 力 。 因 此 ， 一 个 一 个 地 
分 析 机 器 人 连 杆 ， 我 们 以 末端 执行 器 作用 于 环境 的 力学 系统 结束 。 

利用 正 向 或 逆向 递归 牛顿 - 欧 拉 运动 方程 要 取决 于 机 器 人 的 测量 传 感 系统 。 

Ww GI 12-10 2R 平面 机 械 手 的 递归 动力 学 。 

考虑 图 12-11 所 示 的 2R 平面 机 械 手 。 机 械 手 在 端点 处 带 有 一 个 力学 系统 。 我 们 利用 该 
机 械 手 说 明 怎 样 一 步 一 步 地 推导 机 器 人 的 动力 学 方程 。 



























































mg 


mig 


m 


图 12-11 在 端点 处 携带 一 载荷 的 2R F LA 














对 于 第 1 根 连 杆 ， 其 逆向 递归 牛顿 - 欧 拉 运动 方程 为 











F=! Fie X! Fatmilal=! FIi—mi!lg+milal (12-182) 
1M =| Mi — 2 Mao do rr OX Fy HC dr OX Fi + Ilai tio X'h) oi 
=IMi—ini XP et Pt Tey to XT 1 (12-183) 

对 于 第 2 根 连 杆 ， 其 逆向 递归 牛顿 - 欧 拉 运动 方程 为 
2F =’ F D>? Fa tm, a: =—m2*g—*F +m. a, (12-184) 
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Mi= Ms > Mo Cdi ro) X°F + Cd —*r:)X F + la +5@2 X712,50> 
he ae n,X?F,—’m,X’F .4°13a@,+2@, XI o. (12-185) 


机 械 手 由 两 个 R || ROO°) 连 杆 构成 ， 因 此 其 变换 矩阵 T, 是 式 (5-32) 中 的 类 型 。 用 
di 一 0 和 ai 一 0 取代， 则 有 下 列 变换 矩阵 : 
























































cosO1 一 Sin01 0 /icosOl 
sing | cos 0 {1sin01 
T,= (12-186) 
0 0 1 0 
0 0 0 1 
cosĝłĝ2 一 Sin0， 0 lzcos02 
i sind» cos, 0 lesinds 
T,= (12-187) 
0 0 1 0 
0 0 0 1 
齐 次 惯性 矩阵 为 
0 0 0 0 0 0 0 0 
， mili]0 1 0 0 2 mzl2|0 1 0 0 
! 12|oo100 ? 12|0o0 010 
0 0 0 0 0 0 0 0 
齐 次 惯性 矩 是 将 零 行 和 零 列 添加 于 单位 矩阵 中 而 获得 的 。 
所 涉及 的 位 置 矢量 有 : 
— L172 —1,/2 
1 0 2 0 
n= n= (12-189) 
0 0 
0 0 
11/2 l2/2 
| 0 | ， 0 
m= m = (12-190) 
0 0 
0 0 
Ir 1 一 一 II 27r > 一 一 2 1 十 2 一 272， (12-191) 
角速度 和 角 加 速度 为 
0 
0 
Oi=| .| iw@,—|. . (12-192) 
A, 0: +02 
0 0 
0 0 
0| 0 
jx1 一 | aS) .. (12-193) 
01 4, +02 
0 0 
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Ci 的 平 动 加 速度 为 


0 


a, =ha@,X(—'m,)+ho@,Xll@,X(—'m,)]4) d: 


因 为 
1d 1 一 21a ， 


C2 的 平 动 加 速度 为 


2az=2a:X(—? m:) +2 2X2 2X (—? m,) +2 d= 


因为 


在 连 杆 坐标 系 中 ， 重 力 加 速度 为 





—gsin(0ı +62) 
s 7 gcos(@, +02) 
2g =T! % = 
0 


0 











—11 62/2 
=| 1101/2 | (12-194) 
0 
0 
(12-195) 


一 22 (0 十 0 )272 
12 (01 +02)/2 


0 

0 
(12-196) 
(12-197) 
(12-198) 
(12-199) 


在 全 局 坐标 系 中 ， 外 部 载荷 通常 是 已 知 的 。 我 们 必须 将 其 转换 至 感 兴 趣 的 连 杆 坐标 系 
中 ， 以 应 用 递归 运动 方程 。 因 此 ， 表 述 在 坐标 系 Bo 之 中 的 外 部 载荷 系统 为 








下 cos(CO1 十 02 ) 十 下 osin(CO1 4 
F «y cos(O; +02) 一 下 Sin(CO1 7 





ST = 
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现在 ， 从 最 后 的 连 杆 开始 计算 其 作用 力 系统 。 连 杆 (2) 的 向 后 牛顿 方程 为 
oP ie 
FI=—m2 g—? F.tm234a2= Ti (12-202) 
0 


0 





1 ae 
2 Fy, 一 gp lame (6, +42)? 一 下 cos(Ol TO= ey —m2g)sin(@; +62) (12-203) 


1 š : 
2 Fiy =z lam (61 +62)? + Fer cos(1 +62) — (Fey +m2g cos(@1 +62) (12-204) 


ERP (2) 的 道 向 欧 拉 方程 为 


0 
2 2 2 2 2 2 2 2 2 2 2 0 
MI 一 一 M 一 mX’ F.— n,X° F(t °Is6 & ti @:X I5 0:= 2M 
lz 
0 
(12-205) 


ce... ges. a8 l 
2Mi =—Me— 12 Fer sin(O1 +02) — 12 F ey cos: +62) + lam (01 +02)— 3 glomzcos(O) +62) 











(12-206) 
最 后 ， 连 杆 (1) 上 的 作用 力 为 
Fog 
“E 
F=! F,—m,' g tmil a; ='T; Fi — mı! gt+mijai= me (12-207) 
0 
0 
这 里 
i l 1 af bat 1 
Fox =— Fes cosbi ~ (Fey ~mig) sind: ~>l2m2 (01 +02 )sin02 — z gl2m2 
g : 1 。 
(01 +02)? cosô2 十 mgsin(O +202) Flim oF (12-208) 
f , Ï TE 1 
Foy =Fex sind — (Fey Fmi g)coshi +- l2m2 (01 +82) cosb2 —-5 glzmz 
. . 1 。。 
(01 十 02)2sin0s —m2gcos(01 +202) +—=—lim, 01 (12-209) 


2 
连 杆 C1) 上 的 作用 力矩 为 
1M =IM i — Oe ot im XFIT dato XT oi 





0 
0 
'M o; 
0 
(12-210) 


='7,°7M,—'n,X!Fot!m,X!7T,2F tba, tie, X! jø, = 
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这 里 
(Mo =-M +m Gi 十 的) 十 本 mi Ay —(Folst glama)eos(O! +62)— 
Flimigeos0 十 Fulasin(Oi +02) (12-211) 
Kw 12-11 执行 器 的 力 和 力矩 。 
应 用 向 后 递归 力学 分 析 ， 最 后 以 节点 处 的 一 组 已 知 力学 系统 而 结束 。 每 个 结 点 由 一 个 电 


























动机 所 驱动 ， 该 电动 机 作为 一 个 执行 器 向 节点 了 施加 一 作用 力 ， 或 者 向 旋转 关节 R 施加 一 
转 矩 。 当 节点 ;是 平 动 节点 时 ， 驱 动 执行 器 的 力 是 沿 着 z;-1 轴 的 。 
Fa =° ÈT PF, (12-212) 
式 (12-212) 表明 ， 关 节 力 F; 的 &;-1 部 分 由 执行 器 所 决定 ; 关节 力 F ;的 i;-1 和 j;-1 部 分 必须 
由 关节 轴承 确定 。 类 似 地 ， 如 果 关 节 i 是 旋转 关节 ， 则 驱动 执行 器 的 转 矩 也 是 沿 着 z;_1 轴 的 。 
Mn 一 "AL M: (12-213) 
式 (12-213) 表明 : 关节 转 矩 M ;的 & ;部 分 由 执行 器 所 决定 ; eM ;的 i;-1 和 
J ;1 部 分 必须 由 关节 轴承 确定 。 






































12.3 机 器 人 拉 格 朗 日 动力 学 














拉 格 朗 日 运动 函数 提供 了 获取 机 器 人 动力 学 方程 的 系统 方法 。 拉 格 朗 日 浮 数 是 以 运动 动 














AER =N (12-214) 
应 用 拉 格 朗 日 函数 可 以 求 得 机 如 人 系统 的 拉 格 天 日 运动 方程 。 
d(9L) a” | 
‘ae Iq; Qi i5l,2en (12-215) 


这 里 ，g; 是 表达 能 量 的 坐标 ，Q; 是 驱动 g; 的 相应 广义 力 。 
对 于 ” 根 连 杆 串联 的 机 械 手 而 言 ， 其 运动 方程 可 以 矩阵 形式 建立 。 





D(q)q +H(q -q)+G (q)=@ (12-216) 
或 者 
D(q)q +C(q ,gg tG (q)=Q (12-217) 
或 者 以 求 和 形式 建立 
Xp, eS ia dm +G; =Q; (12-218) 
k=lm=1 


Di 是 一 个 nXn 惯性 系统 项 ， 即 








n 1 
Di; = > (Fem to + Fhe Td | (12-219) 
k=1 
H ip 是 速度 看 合 项 ， 即 
n n 3D; ID; 
H=), ee (12-220) 
l j=1k=1 dqr 2 9gi 
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G: 是 重力 项 ， 即 
G; = Sa I (12-221) 
iE AA: = 
连 杆 (i) 的 动能 
Ki=>° vim v; ~ CE RCE (12-222) 
这 里 ，m; EERE, I, dE ERT A hp A P EFF EE PE o ;是 在 质心 C 处 连 杆 的 全 











局 速度 ,00 ;是 连 杆 的 全 局 角速度 。 














基于 关节 坐标 速度 ， 利用 连 杆 的 雅 可 比 和 矩阵 J ;可 以 表示 平 动 速度 矢量 和 角速度 矢量 。 
. (v; Joi). 5 
xf, js q—Jiq (12-223) 
QO; Ri 
JERE ASAE FT LEME J: 是 一 个 6Xn 和 矩阵 ， 该 矩阵 能 够 将 瞬时 关节 坐标 速度 转换 成 瞬时 连 杆 的 
平 动 速度 和 和 角速度。 对 于 js 入; ， 雅 可 比 和 矩阵 J: 的 第 7 列 由 ce g 和 e 也 构成 。 
í by X? yr; (对 于 R 关节 ) 
cw =|; (12-224) 
kj- OFF P 关节 ) 
o kja (对 于 RR 关节 ) 
eH = (12-225) 


0 
j=l i 


雅 可 比 和 矩阵 J: 的 列 是 零 。 





0 (对 于 P 了 关节 ) 


是 坐标 系 Bj-1 中 且 表 述 在 基体 坐标 系 中 连 杆 (i) 的 质心 C 的 位 置 矢量 。 对 于 >i, 


这 时 ,机 器 人 的 整体 动能 K 为 


K= DK, =F a (v? m; ets 


z wl 





oH OT, Æ G) 心 


| 
see 


To @ | 


(Jp: qi) m; Jo: q: + Jr GH, (Ji qi | 








机 器 人 整体 动能 可 以 写成 一 个 更 加 简洁 的 形式 ， 即 





RE, D 是 一 个 被 称 为 机 械 手 惯性 矩阵 的 2 义 ? 


D= 


E 力 而 产生 的 连 杆 (i) 势能 











Ha 








因此 ， 机 械 手 的 总 势能 为 


J BbimiJ pi + ss Ri TiJ Ri Jle: (12-226) 
且 表 述 在 基体 坐标 系 的 惯性 矩阵 。 
°I;=°R;'L; R] (12-227) 
1 了， ， 

K=-74q; Da; (12-228) 
Bm Joi + SJE TF) (12-229) 
i=1 

ea ty. (12-230) 
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V= > Vi 一 一 >ymi0gTOr， (12-231) 
i=1 ;一 1 
这 里 ,"g 是 表述 在 基体 坐标 系 中 的 重力 加 速度 矢量 。 
HLR F HIHI BH H KRO 
I: ‘ z o il 
= RV Dd 2 re FID di di + Sim's rs 
i=1 j=l j=l 
(12-232) 
Eon 数 ， 我 们 可 以 求 得 
aL 3r; 1 ", 232D 
J 9 9 > qe) + Foou 7 一 pe ig. © qe POEL 
dg ° qi j=1k j=l qi 2 52 bal qi 
(12-233) 
ag n . 
-=D Di gi (12-234) 
an j=l 
d (af 2 Dj. ID jj; . 
=>) Dijqj + a Aq = = © Dyg + pe Bo dedi (12-235) 
dt aqi j=l j= 
拉 格 明日 方程 的 总 合成 为 
Qi=Mi+J'F. (12-236) 


XE, M; 是 在 关节 ; 处 第 i 个 执行 器 的 力 , Fe=(—-F i, 一 M 2)? 是 作用 于 末端 执行 器 
上 的 外 部 力 系统 。 
最 后 ， 对 于 一 个 具有 个 连 杆 的 机 械 手 来 说 ， 其 拉 格 半日 运动 方程 为 

















SD: (q) Gj + H iam qr qn + Gi =Q; (12-237) 
其 中 ， 和 
Hin= 2 5 | (12-238) 
G;= Se og TJE (12-239) 
我 们 可 以 使 机 械 手 的 运 za 动 方程 以 更 加 简洁 的 形式 简化 息 阵 计算 ， 
D(q q +H (q ,gq ) 十 C (q )=Q (12-240) 


mop} 


选项 G (q) WENE 
时 可 以 被 写成 下 列 形 式 。 





HRE, ERM q. q) 被 称 为 速度 耦合 矢量 。 速 度 耦 合 矢量 有 








H (q -q)=C (q -q)q (12-241) 
例 12-12 平 动 -转动 平面 机 械 手 。 
图 12-12 所 示 为 一 个 具有 无 质量 连 杆 和 两 个 质量 点 m1 和 m2 的 平面 机 械 手 。 为 了 确定 
运动 方程 ， 我 们 从 计算 动能 开始 ，。 























1 P 
Kı=5mı nad (12-242) 
1 3 1 : 1 d 2 A d . 2 
Kis ym X21+ yma Y= pmo] oC -Leosq2) | + ma] asing) | 
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m 














\ 
2 
C oe 
Lis 
ti 
图 12-12 平 动 -转动 平面 机 械 手 
E e e 1 . ， 
= sme (qi L qzsingz )’ +> m2 qzcosqz)? 
1 ; po ai E xd 
= ym (qi +1? q3 — 2l g1g2 sings) (12-243) 
机 械 手 的 势能 为 
V=m2gY2=mzeglsing2 (12-244) 


因此 ， 拉 格 时 日 函数 为 





1 i 1 z ‘i . : 
=K—-V=Ki+K2—-V= 5m qi ymagi tl? qz—2l dıdzsingz)—mzglsingz 











(12-245) 
应 用 拉 格 明日 方程 : 
da2 ag f 
als) io; Qi i=1,2 (12-246) 
可 得 下 列 运 动 方 程 : 
(mi1+m2)91—m2l gz2sing: —m2l q} cosd2 一 QIl (12-247) 
mol? qi —mİ qı sing2 +m2glcosq2=Q2 (12-248) 
将 这 些 方程 重新 调整 成 式 (12-217) 的 形式 。 
qı .fgi Qi 
D(q)|.. [+C .q)| , |+G(qd= (12-249) 
q2 q2 Q: 
这 里 
mı +m? —moelsings 
D(q)= . (12-250) 
mol? —m2lsing2 
: 0 —m2lg. 2 
Cag »-| oe atl (12-251) 
0 
c@-| | (12-252) 
m2 glcosq2 





例 12-13 平面 极 坐标 机 械 手 。 
12-13 所 示 为 一 个 具有 无 质量 连 杆 和 质点 m 的 平面 极 坐 标 机 械 手 。 
机 械 手 的 动能 》 


























1 $ 1 : 
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Sa ») y 
= z T qı cosq2 
1 (d. 2 j 
+35: sings | 
i 
mai tay q2) (12-253) i 
2 
机 械 手 的 势能 》 pa 
V=mgY2=megqising2 (12-254) 
因此 ， 机 械 手 的 拉 格 明日 函数 为 图 12-13 平面 极 坐标 机 械 手 
$=K—-V= > md tg? q3)—megqising2 (12-255) 
应 用 拉 格 明日 方程 
d(9L\ av 
; Q; i=1,2 (12-256) 
a aqi í 
可 得 下 列 运动 方程 
m gi—mqi q3 tmgsing2 =Q: (12-257) 
mq? qz +2mqı qıq2 t+mgqicosq2 =Q2 (12-258) 
将 这 些 方程 重新 调整 成 式 (12-217) WÉR., A 
q (qi Q 
D@| | |+C(q.q fE jew- | (12-259) 
q2 q2 Qz 
这 里 
D(q) -(. | (12-260) 
Clg,g | = (12-261) 
md 1 q? mqı qı 


G @q -| ng | (12-262) 


mgqıcosq2 





例 12-14 具有 质量 臂 的 2R 机 械 手 的 拉 格 朗 日 方程 。 
2R 机 械 手 如 图 12-14 所 示 。 其 齐 次 变换 矩阵 在 式 (5-29) Ash (5-30) 中 已 经 给 定 














cos01 一 Sin01 0 
°R, =| sind, cos0; 0 (12-263) 
0 0 1 
cos? 一 Sin0> 0 
'R,=|sinO2 cosg，， 0 (12-264) 
0 0 1 
假设 每 根 连 杆 中 的 材料 构成 相同 ， 质 心 Ci 的 位 置 矢量 为 
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17/2 
ir;=| 0 i=1,2 (12-265) 
0 
其 惯性 和 矩阵 为 
0 0 
iI; Smil 1 0 (12-266) 
0 0 1 
因此 ， 有 
sin201 一 cosOl sinfı 0 
oT RT R] =m} 一 cosOl sind cos201 0 (12-267) 
0 0 1 
sin201， 一 cosOlysin0l， 0 
Ty =° R, I R] = mil} —cos612 sinf12 cos? 012 0 (12-268) 
0 0 1 
假设 重力 位 于 一 记 方 向 
0 
g =|—g (12-269) 
0 
连 杆 的 雅 可 比 和 矩阵 为 
-sinb 0 
Jo 一 cei j (12-270) 
2 
0 0 
0 0 
JRI 王 |0 0 (12-271) 
1 0 


463 


应 用 机 SI 人 


学 : 运动 学 、 动 力学 与 控制 技术 














1 
— sind; ~ 52 sinf12 py elesind 2 
2 一 1 1 (12-272 
Jo — lı cosl + z l2 COS0 12 z2 C08s012 ) 
0 0 
0 0 
Jr2=|0 0 (12-273) 
1 0 
替换 式 (12-229) 中 的 "五 、Jp 和 JR ， 我 们 可 以 计算 机 械 手 的 惯性 矩阵 。 
1 
D = D (Thm +4 FBLA ws =I im ids + FR Ja +i 
1 一 1 
1 2 3 1 3 1 Taz 
] gil me Li +lilzcosé2 + -l m2 g fibzcosda 十 本 /2 
十 可 Ji IJR = ] ] 1 
ma(Glilacosde +315) mal} 
(12-274) 
速度 耦合 矢量 H 有 两 个 部 分 
让 i 
aD aD; g 
Hi=)) > i aa qk mali hf +52) Posing: (12-275) 
j=1k=1 dqr 2 qı 2 
2 AOD; 1 oD 1 
H: = 4 — Nd =—mol\lo 0 0 12-276 
2 HE 2 a g z abide Gy Sing ( ) 
重力 矢量 G 的 两 个 部 分 分 别 为 
1 1 
Gi=zmiglicosd; tmz gli cosh +> m2 gl2cos012 (12-277) 
G: =— mzglzcoslı2 (12-278) 


2 


现在 ， 我 们 可 以 组 合 形成 2R 平面 机 械 手 的 运动 方程 。 假 设 末 端 执 行 器 上 没有 外 力 ， 


动 方程 为 





1 
Qi [Bm | 


molile(t+ 5th Józsi + [二 mits Jat cosh + m2 gl2cosbi2 


1 
Qi=m:2 [Fs Lacost: 

















1 yi 1 1 i 
mfi FL, l2cos02 +518) [a + mala (Zh cost +32 |B 


(12-279) 


dt 535, Vest 1 > 1 so 1 
+503 )h+ pmol 02 + Fm2lil2 Ji sinz + me glecoshiz 


























(12-280) 
*% 12-15 EIEI (Christoffel) AT, 
符号 ,被 称 为 克 里 斯 托 费 尔 符号 ,或 者 克 里 斯 托 费 尔 算 子 ， 其 定义 为 
; 1faDs ,aDa aDn | 
” ae on a (12-281) 
速度 耦合 矢量 Hj 就 一 个 克 里 斯 托 费 尔 算 子 。 
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7 n n ID ij 1 ID jx -a 1 n n JD.. aD, aD. 
Hi = > ic - zx) >n > ik Pi | (12-282) 
F a = Idk ogj Odi 


利用 克 里 斯 托 费 尔 算 子 ， RATT AE a Pa La Ae oT TE 


Di (qd aj 十 > > Fs dk qm HG; =Q; (12-283) 
j=l 


j=lk=l1 
KB 12-16 无 重力 和 无 外 力 。 
假设 无 重力 和 无 外 力 应 用 于 机 器 人 的 末端 执行 器 。 在 这 些 条 件 下 ， 机 械 手 的 拉 格 朗 日 函 
数 可 简化 为 














ji n n 。 。 
= SDs di a (12-284) 
i=lj=1 
运动 方程 简化 为 
d/o97 ag L oč $ a 
le Fg 2 Dy Di Gi ATi mdi dm) (12-285) 
“da i i=l j=1 


例 12-17) 单 连 杆 机 械 手 的 拉 格 朗 日 方程 。 

相 比 牛顿 - 欧 拉 法 ， 为 了 展示 拉 格 朗 日 方程 的 优点 和 简洁 性 ， 让 我 们 考虑 在 末梢 点 处 得 
到 具有 质量 m 的 图 12-15 中 所 示 的 均匀 悬臂 梁 的 运动 方程 。 这 是 与 图 12-6a 所 示 的 相同 
系统 。 

在 矢量 "ri 处 ， 具 有 质心 的 粱 是 均匀 的 ， 与 此 同时 末梢 质量 在 "di 处 ， 它 们 都 在 坐标 系 Bo 
之 中 。 


























L cosp 
r= Riri=|i . (12-286) 
sinf 
0 
/coSsO 
od =°R,'d, =| lsin (12-287) 
0 
cosO 一 Sin0 0 
"及 ,一 | sin0 cos? 0 (12-288) 图 12-15 ”在 末梢 点 处 具有 挂 码 mo 
0 0 1 的 一 个 均匀 梁 
梁 的 角速度 为 
ow01=0K (12-289) 
因此 ， 质 心 C 和 ms 的 速度 为 
一 了 jsing 
0y1 一 0O1X71 一 | 1. (12-290) 
z? COSO 
0 
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—16 sind 
°di =o @iXW@i=| jg cosg (12-291) 
0 





机 械 手 的 动能 ; 


1 。 。 1 1 I 1 4 
Ks= Fm2 di “di +t ami ml ; wit F001 0@ 1 =e 0 thdm) ts 10 


(12-292) 
I 0 0 T,cos’?@+Tysin?@ (I,—I,)cos@sind 0 
011 =Rz w Ti RZ9=°Ri| 0 I, 0 | °RE=|(z—I,)cos@sind Tvcos20 二 Tsin20 0 
0 QO Te 0 0 I. 

(12-293) 


机 械 手 的 势能 》 
L 
V=migYitmegY2=migry tmogdy=mig g sind + m2 gl sind (12-294) 


因此 ， 机 械 手 的 拉 格 朗 日 函数 为 




















1 ., 1 : Zl, . 
2=K—-V= 0? (m, 上 4772 > ) 二 31=0° mig z sind m2 glsind (12-295) 
应 用 拉 格 朗 日 方程 
fat) 2 (12-296) 
dt a6 gg 20 a 
Ag 1 ‘ ` 
“Z= (mı 4m2) +I- (12-297) 
ap. 4 
d (ag 1 j 
a J= m tml? r.) (12-298) 
30 
Iy L 
39718 z 6088 mz glcosd (12-299) 
可 得 运动 方程 为 
fl 2 2 N | 
Qo qe tmol? +I: 10 4 z771 Hmo |glcosé (12-300) 


该 式 与 式 (12-69) 相同 。 

例 12-18 2R 平面 机 械 手 的 常规 模型 。 

考虑 一 个 具有 质心 机 械 辟 和 关节 的 常规 2R 机 械 手 ， 与 此 同时 承载 有 效 载荷 mo, WE 
12-16 所 示 。 

驱动 连 杆 (1) 的 第 1 个 电动 机 位 于 地 基 上 。 具 有 质量 m 的 第 2 个 电动 机 驱动 连 杆 
(2)， 安 装 在 连 杆 (1) 上 。 第 1 个 连 杆 和 第 2 个 连 杆 的 质量 分 别 是 mi 和 12。 

通常 情况 下 ,质心 C 和 有 质量 关节 的 全 局 位 置 矢量 为 
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图 12-16 
Cc 
rı =° R1! rı =Rz, cili 5] c 
Org = "di +°Ro? re 一 0d1 十 Rz7o 
L 


odi =° Riri 5Rz.oa lilii = L 


Lı cos 4 


1COSO1 
1 sing; 
0 


L1c0s011 


4 有 质量 臂 并 携带 有 效 载荷 mo 的 2R 机 械 手 





1Rz,0c2 12 = | li sinfi -+ 


cos01 
sing, 
0 
-l2 cos(01 +02) 





od =°d; +°R2?d2=| lisin ṣ4 


tZosin(@; +02) 











0 
这 里 

cos#; 一 Sin01 0 

OR: SRZ., =| sind, cos 0 

0 0 1 

cosO —sinéz 0 

"R.=Rz.9, =| sinf? cos0» 0 

0 0 1 
cos(0ı +42) sin(@; +02) 
OR =9R,!Ro=| sin(O; +62)  cos(@, +82) 

0 
连 杆 的 角速度 为 
001 一 01 开 





0@2=(6,+0.)K 


(12-301) 
tc 1 cos(@; +02) 
rc sin(@; +02) 
0 
(12-302) 
(12-303) 
(12-304) 
(12-305) 
(12-306) 
0 
0 (12-307) 
1 
(12-308) 
(12-309) 
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全 局 坐标 系 中 的 惯性 矩阵 为 
ES 0 0 
°T) =Rz.9,'EiRZ9,=9Ri| 0 Ty 0 | ORT 
0 0 Ia (12-310) 
Taulicos201 十 Tisin201 (CT 一 Ti)cosOisin0 0 
=| (Ta 一 Ti)cosOisin01 Ticos201 十 Talisin201 0 
0 0 Iz 
Ti2 0 0 
°Ts =° R? IR] =R; 0 Tyo 0 oR 
0 0 fz (12-311) 
Tazzcos2012 Ty2sin? O12 (I z2 — I y2)cos012sinĝ12 0 
= (142 —Iy2)cos12 sind 12 T,2cos? 012 +12 sin? 412 0 
0 0 IT .2 
012 =01 +62 (12-312) 
质心 C; 和 机 械 臂 质量 的 速度 为 
， 一 c10 isin) 
v= rS] 616 i cosh: (12-313a) 
0 
ee —1101sin01 —c2 (01+02)sin(01 +42) 
0 . . . 
"v? ge 1,0, cos0, +c (01 +4 2)cos(0ı +42) (12-313b) 
0 
—110 isin: 
°di=| 1 8, cos: (12-314) 
0 
—1,6 sind; —l2 (1 +82)sin(0ı +42) 
°d2=| 116 cos0i +l (61 +62)c08(61 +02) (12-315) 
0 





为 了 计算 拉 格 朗 日 函数 X==K 一 V， 
EX 


4 
H 


il. 。 。 
K= min d,-°di- 


我 们 需要 确定 机 械 手 的 动能 和 势能 。 机 械 手 的 动 


] 0 
Cami Vi”. 


1 。 。 
° ait Sino? do +9 dat 


(12-316) 





5 3 O yL 
V2 V2 


1 
5 0@1°Li0 O: +007 Izo 0? 


将 式 (12-305) ~st (12-315) 代入 后 ， 则 有 
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1 . 1 i ; ‘ 
K=; (muci tml F118 1 + Smal 118 1 sind — e291 +82) sin, +2)? 


1 š 
十 到 moL 一 人 0 sinb: 一 /7200 1 +6 >)sin(O; +42) |? 


+ smo [116 1COSO1 +1,(6 1 +Â > )cos(0ı +02) |? 
1 





HaTe +02) ? 
(12-317) 
机 械 手 的 势能 》 
V=mugcisin0 +mi2gli sind; 十 21gLLisin0l 十 czsin(O +62) | (12-318) 
十 72ogL2isin0l +l2sin(@; +42) | 












































应 用 拉 格 朗 日 方程 

d | | aL 
g Qo (12-319) 

dz a01 901 

d | 22) Iy 
A Qi (12-320) 

dt Iĝ 302 

以 此 确定 广义 运动 方程 为 
Di, Di 区 si Ci Ci? h + 上 
Da D22/\0, Car C22)\0, G2) (Qi 
Dy, =21, (mz21c2 +mol2)cosd2 | IESI | T .2 | mc? Fm127? | mai (c? | 1?) Hmo Cl? | 13) 

(12-322) 
Di2 =li (mcz +mol2)cosd2 了 -> mol? Fmac? (12-323) 
Dy =L) Gn21¢2 +mol2)cosé2 T 22 macs | mol? (12-324) 
Do =I z2 +mac? T+mol? (12-325) 
Cy =— lı (mz21c2 tmol2)6 2sind2 (12-326) 
Ci 一 一 0 (macz +mol2)(ĝ1 +62) sinds (12-327) 
Co =11 macs +mol2)Ô sinb? (12-328) 
C22 =0 (12-329) 
Gy =[ (m2 Fmi: #mo)lı Hmi1cı ]eos8; + (mocs mols)cos(01 +02) (12-330) 
G2 =(m21c2 +molz)cos(O; +02) (12-331) 





例 12-19 2R 平 面 机 械 手 的 特殊 情况 。 

12-16 所 示 为 具有 质量 的 机 械 臂 和 关节 以 及 所 携带 有 效 载荷 mo 的 常规 2R 平 面 机 械 
手 。 第 2 个 电动 机 的 质量 为 m, KREEF (1) 上 。 第 1 个 和 第 2 个 连 杆 的 质量 分 别 是 
m1ul 和 m2z1， 其 质心 分 别 位 于 C1 和 C, 2R 平面 机 械 手 的 党 规 运动 方程 在 式 (12-321) 中 已 


Aez 
给 定 。 








Du Day if? Ch Cei G 
| ii ") 2 H 11 i +| R) (49-3505 
Da D2)\0, Car C22) 6, G? Qı 
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在 建 模 一 个 特殊 的 2R 平面 机 械 手 中 ， 我们 可 以 使 用 方程 对 于 较 简 单 的 模型 进行 如 下 
分 类 。 

1) 无 质量 机 械 臂 。 当 机 械 手 中 连 杆 的 质量 小 于 其 电动 机 和 承载 载荷 的 质量 时 ， 我 们 可 
以 使 用 一 个 无 质量 机 械 臂 模型 。 对 于 无 质量 机 械 臂 的 2R 平面 机 械 手 来 说 ， 其 运动 方程 将 
mı =0, ma =0 RAK (12-321) 中 进行 计算 。 




































































Di =2molil2cos02 +11 +122 mili +mo Uj +13) (12-333) 
Di2=molil2c0s02 +I 22 mol? (12-334) 

D21 =mol,l2cos82 I 22 mol? (12-335) 

Do. =122+mol3 (12-336) 

Ci =— mol 1126 2 sinde (12-337) 
Ciz=—molil2(81+62) sings (12-338) 

Cor =mo1 1126 1 sinde (12-339) 

C22 =0 (12-340) 

Gi = (mo +miz2)l1cos81 +mol2cos(@1 +42) (12-341) 
G2=mol2cos(@, +02) (12-342) 





2) 无 质量 关节 。 U E ae el Oa E 量 时 ， 我 们 可 以 
使 用 无 质量 关节 模型 。 对 于 无 质量 关节 的 2R 平面 机 械 手 来 说 ， 其 运动 方程 可 将 mp =0, 
mo =0 代入 式 (12-321) 中 进行 计算 。 


























Dı = 2m21l1c2c0s02 | Tey | 了 -2 | mc? | mM 2) (c? +) (12-343) 
Diz =m21c2 (11 cosd2 c2) 了 -2 (12-344) 
D21 =5m21c2 (l1 cos82 +ce2) +1 22 (12-345) 
D22=I1.2+m21¢3 (12-346) 
Cu=—malic262sind2 (12-347) 
Ci2=—ml1 C2 (6, +6>)sind» (12-348) 
Co =m 11026 1 sinde (12-349) 
Coo =0 (12-350) 
G1, =(m 1.614 +m, L1 ) cos8, +m21¢2c08(8, +02) (12-351) 
G2=m21c2c0s(O, +82) (12-352) 
如 果 机 械 手 的 连 杆 是 均匀 和 对 称 的 ,那么 c1 二 11/2、c2 二 12/2， 各 参数 可 简化 为 
1 
Di =mailil2cos6, HI +1 2244 gruti tma Hi) (12-353) 
1 
D2 = sma la[t costs +51 ~ (12-354) 
1 
D: = pmeililecosd, +I. 这 于 二 fale (12-355) 
] 2 
D2 =1:2 tT Tm2l? (12-356) 
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sE NEADO E 
1 ae 
Cu =— smalil20 sing (12-357) 
1 š . 
Ca =—ymailil, (G1 18 2)sind2 (12-358) 
1 a 
Cai =7mailil28 1 sings (12-359) 
Co. =0 (12-360) 
1 1 
Gi = (mn Hm Jcos +5maitecos(i +02) (12-361) 
1 
Gy=—=m2l2cos(O, +62) (12-362) 


2 


KB 12-20 关节 机 械 手 的 运动 方程 。 
12-17 所 示 为 一 个 在 末梢 点 处 具有 质量 连 杆 和 载荷 的 关节 机 械 手 。 点 Ci (i 二 1,2,3) 说 明了 





m 





图 12-17 在 末梢 点 处 





具有 质量 m; (i 二 1,2,3) 连 杆 的 质心 。 末 梢 端点 的 质量 为 mo。。 机 械 手 的 俯视 图 如 图 12-18 所 示 。 



































tk 有 质量 连 杆 和 质量 载荷 的 关节 机 械 手 


机 械 手 的 连 杆 (1)〉 是 一 个 R FFR(C90")， 并 沿 着 zl 方向 有 一 个 额外 位 移 2; 的 连 杆 。 为 





转 和 移动 将 坐标 系 Bi 移动 到 当前 状态 。 

















了 确定 变换 和 矩阵"R1， 我 们 从 坐标 系 Bi 和 坐标 系 Bo 的 重合 情形 开始 ， 通 过 一 系列 正确 的 旋 
cOSO1 sin01 0 0 
0 0 1 0 
1T) =D, ,nN Rx ,n/2R 2,0; = (12-363) 
sind; 一 cos0O 0 2, 
0 0 0 1 
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图 12-18 在 末梢 点 处 具有 质量 连 杆 和 质量 载荷 的 关节 机 械 手 俯视 图 
cosO1 0 sinfı —l\sin@, 
四 sind; 0 一 cosOl Z1cosO1 
Tı =!T3! (12-364) 
0 1 0 0 
0 0 0 1 
第 2 和 第 3 根 连 杆 是 RRO)、R FR(C90") 连 杆 ， 其 坐标 系 之 间 的 相应 变换 矩阵 为 
cos0» 一 Sin0， 0 /cosO， 
sin® 2 cos 2 0 lesin€2 
IT, = (12-365) 
0 0 1 0 
0 0 0 1 
cosO3 0 sins 0 
sind; 0 —cosé3 0 
2T; 一 (12-366) 
0 1 0 0 
0 0 0 1 
质心 C; 和 关节 的 全 局 位 置 矢 量 为 
0 一 (01 一 cl)sin01 
0 (1 一 cl)cosO1 
0r1 一 0T11 7r1 一 07T) 一 (12-367) 
C1 0 
1 1 
0 —1,sin@, 
0 lı cos6\ 
0d, =—9T1!'di =°Ti = (12-368) 
0 0 
1 1 
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Sve meine Gi 





— 2l sinfı + (l2 +c2)cos0) cosh? 
or = di HT? r= 2lı cos + Cl SPOE ANPE (12-369) 
(> 十 cy )Ssin0， 
2 
2l cos cosO2 — 21; sind; 
od: =d, +T? d; = a nae AEROS TET 
212 sin02 
2 
— 31, sinfı +c3cosd; (sin(@2 +43) +3l2cos@2 ) 
op, =) do HOT? ry = 31, cos0; Tesini Csin (02 +03 )+3L2cos02) Gsi 
3l2sinf2 —c3cos(02 +03) 
3 
—3lı sinfı +13cosd; (sin(02 +03) +312 cos02) 


31, cos@; +23 sin@; (sin(@2 十 03 ) +3Ll2cos02) 











od; =° d +°T3° d3= (12-372) 
: i e 322 sinz — /3cos(0, 十 03 ) 
3 
连 杆 的 角速度 为 
o@1 =01k0 1 @2=6 2k) 2 @3 =0 sk (12-373) 
0 一 0 一 0 ,cosO1 
082 =061 +402 一 0G1 +°R1102°R] 一 a, 0 —@6 sin, | (12-374) 
Ê > cos; Ê> sinô: 0 
0 =); —(8,+63)coshi 
003 =0@2 +503 =0@2 +° Ro2O2R} = Ay 0 —(82+63)sind, 
(62+63)cosh) (0 3 +03)sing! 0 
(12-375) 
全 局 坐标 系 中 的 惯性 矩阵 为 
Ti 0 0 
oT; =R, 0 Iy 0 ORT 
0 0 Ta (12-376) 
Ticos201 十 Tsin201 (I1 —I21)cos6) sing, 
=| (I1 — I z1 )cosĝı sind; IŁzıcos?0ı +I x) sin? 4; 0 
0 0 Iyi 
了 -2 0 
oT, =° R? IOR] =° R? 0 Tye 0 oR? (12-377) 
0 0 了 -2 
123 0 0 
013 =9R3313;°RI=9R3| 0 I 0 ORT (12-378) 
0 0 I 23 
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质心 C; 和 关节 的 速度 为 


— (lı > 一 让 )01cos01 


0 


RR Oe ; 
e= s 一 (1 一 c1)01Ssin01 
0 
od 
ve a 
0 
Us = FFs 
—16 1COSO1 
0 了 — 
‘di =| = sink 
0 
ig c= FA 
od 
0 0 
ay, t 


机 械 手 的 动能 > 


il 1 1 1 . 
K=; m"V i "Vt om v2 $ v2 Hms’ vs $ °v Ho mo’d 3 «d; + 


1 1 1 
T T T 
9 01 11001 t F002 L202 F700; T3003 


机 械 手 的 势能 》 
V=m2 grz: +m3er3z +mogd 3 
利用 机 械 手 的 拉 格 朗 日 函数 XK 一 V， 应 用 拉 格 朗 日 方程 


dù 34 Og 
pee 




















dt 96, 901 
d{ dag Og 
em i 
d{ dag Og 
T) 7, Q? 
我 们 可 以 确定 运动 方程 为 
Du Diz Dis 4 1 Ci Crp 


Dn Dz Das || 62/+|Car Cz 
Ds Ds Da3)(G f C31 C32 


K12.4 拉 格 朗 日 方程 和 连 杆 变换 矩阵 


基于 拉 格 天 日 方程 ， 机 器 人 运动 方程 的 矩阵 形式 为 
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(12-379) 


(12-380) 


(12-381) 


(12-382) 


(12-383) 


(12-384) 


(12-385) 


(12-386) 


(12-387) 


(12-388) 


(12-389) 


(12-390) 
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D(q)q@ +H(q.q)+G(q)=@ (12-391) 
sk (12-391) 也 可 以 被 写成 求 和 的 形式 ， 即 


n n 





























Sin YS ait oo, (12-392) 
j=l ll 
ERE D(q) 是 一 个 nnXn 惯性 对 称 和 矩阵 。 
i 3T. — aT! 
Dy= >) uf 2 "I, =) (12-393) 
r=maxi.j aqi 9gj 
H itm Fe TE EERST, 
n D20 — TT 
Hi= 2) ef Te g, "| (12-394) 
r=maxi,j Iq; Idk Idi 
G: 是 重力 矢量 。 
IT 
=S (12-395) 
r=i A 
UE AR: 


在 刚体 坐标 系 B 中 ， 矢 量 irp 处 连 杆 G) 上 一 点 乙 的 位 置 矢量 可 以 通过 式 (12-396) 
被 转换 到 基体 坐标 系 之 中 。 

















Opp =T;' rp (12-396) 
因此 ， 在 基体 坐标 系 中 ， | 
OT; 
OF p= 5 — į jirp (12-397) 
s= 43 
j=l 
| IT; i IT; T 
FL =F, pu FpD =u] DI m dite Dy = qi re) | 
二 1 qj 元 兰 让 Cdk 
oT ,7/9 TT 
mer pale EZA (12-398) 
RF “di qk 
具有 微小 质量 dm 的 点 P 的 动能 








OT IT; Ti : 
dK p -HDD rp! (Se) dude fem 
Idk 


j= Tl TA 


-HD 


j=l1k=1 








aT; T 

“(i rpdm' rp ida (12-399) 
ggk 

ŽIT G) 的 动能 为 


1 i i aT; IT; a 
[othe te BEE nortan (Eee) o 











Iq; a 
j=1k=1 j qk 
对 于 连 杆 G), sk (12-400) 中 的 积分 是 伪 惯 量 矩 阵 [ 式 (11-143), 
iJ; 一 i irpiri dm (12-401) 


KIE, EFF G) 的 动能 变 为 
1 i i oT; - 3? A T 
Kieze > - an (ZE) iia | (12-402) 


RA n REIKIA, AAA 








A 
Ri 
E 
E 
六 
‘ 
5 
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K -PK 7X > iI; 3 | aide | (12-403) 


=) Yatia 29; aq 
由 于 驱动 电动 机 Ka 安装 在 机 器 人 关节 处 ， 因此 也 可 以 动能 相 加 。 




















n 1 Ae, 
2 yiii OFF RW 
ee (12-404) 


n 1 Lie 
5 zigi TF P RH) 


i=l 


KE, L 是 在 关节 i 处 旋转 电动 机 的 惯性 矩阵 ，m; FITA ee. AT. RATE A 
认为 电动 机 质量 集中 在 关节 处 ， 将 关节 i 处 的 电动 机 质量 加 入 到 连 杆 (i 一 1) 的 质量 上 ， 并 
旦 调整 连 杆 的 惯性 参数 。 关 节 i 处 的 电动 机 驱动 连 杆 〈; ) 。 

对 于 势能 ， 我 们 假设 重力 是 势能 的 唯一 来 源 。 因 此 连 杆 (i) 在 基体 坐标 系 中 的 势能 
Vi=—m:° g* ori=—mi" gT} r; (12-405) 
这 里 ,"g 一 (g gy ge 0)T 是 重力 加 速度 ,通常 沿 着 一 zo 方向 ， 矢 量 "r; 是 基体 坐标 系 
HER G) 的 质心 C 位 置 矢 量 。 整 个 机 咒 人 的 势能 等 于 












































V= V; =— |m? gUT;'r; (12-406) 
i=1 i=l 
机 器 人 拉 格 明日 方程 可 将 式 (12-403) 和 式 (12-406) 代入 拉 格 朗 日 方程 (12-214) 中 
便 可 求 得 。 
OT aT; n ; 
f= DD 二] aan Simi? 8gToTiri (12-407) 
i=1j=1k=1 Cdk = 


st (12-407) 应 用 于 将 拉 格 朗 日 方程 [ 式 12-215) ] 之 中 ， 可 求 得 机 器 人 运动 动力 学 
方程 。 我 们 逐 项 地 确定 运动 方程 中 的 每 一 项 。2Z 对 d ,的 导数 等 于 


fo} SSE a) A BEG Yo 









































Jå, 73 k=1 dqr Ide i=1j=1 
n i aT; = aT; T 
=>, GÉ - ae } la (12-408) 
i=r j=l edy Gr 
因为 
aT; 
5 =0 Of Fri) (12-409) 
Cdr 
ƏT; — ƏT; T T; = aT; T 
"| | } [= É iI; | } | (12-410) 
9gj Age Igk aq; 
92/adr- 的 时 间 导 数 为 
ay > ee z a)l n i i E- 到 Con fs 
qi + tr Ti; Fide 
ata) 2 a Iqr a 9qj9qk Iqr 
D20 oaoT. T 
T Se 元 | +} Jaw (12-411) 
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Sve meine Gi 


























拉 格 明日 方程 的 最 后 一 项 是 
ay l n i i = a Con rae 
= tr 1 i P zs 
dq, 2 ijk 9qj;9gr Ie djdk 
lwo "| PE [iit (12-412) 
2 ji Logeogr (3q; E 
n IT; ; 
Ym; T ‘Ti 
i=r gd 
其 可 以 简化 为 
ag B3 ab. É OT; — 1) | IT: , (12-413) 
= tr ‘I didet Simi g2 r; 
dqr ff ar ag; dq ’ = 


有 趣 的 是 ， 式 (12-411) 中 的 第 
方程 中 的 这 些 项 后 ， 可 将 其 简化 为 


d (= ag l 
tr 


dt 


n 了 








gi) 34: IE 


n ï 


最 后 ， TRA n 根 连 杆 的 机 器 人 来 说 ， 其 运动 方程 











go T; 








3 项 等 于 式 (12-413) 中 的 第 ap 因此 取代 拉 格 衣 昌 
YT 


= a Me 
CB) Jet 


aT; a ; 
1,/ i)’ | 一 et 


“| qn 9qm j=i 





Idk 


aT 
daqi 








vn 
(12-414) 


Ši 
FE 


























OT iT a T; JI OT. T kd 
Qi= ey (oe ze e) Ja+ So Ee 5 5 ‘| | and 
i=ik=1 edi j=ik=lm=1 qkm egi 
n aT; . 
domi gt g t 
六 = 9gi 
(12-415) 
运动 方程 可 以 更 加 简洁 的 形式 写成 
Qi = >) Dadi + D>) OHindGtG: (12-416) 
j=l j=lk=l 
这 里 
n IT, = ƏT, | 
D; = t rI, (12-417) 
’ ee | qj - ) 
n 32T, _ E5] 
H jk = tr "I, (12-418) 
j ot ae Iqi 
YT 
- Dmg" r, (12-419) 
TA 
例 12-21 装 在 天 花 板 上 的 2R 机 械 手 。 
图 12-19 所 示 为 一 个 安装 在 天 花 板 上 的 理想 2R 平面 机 械 手 。 在 一 些 机 器 人 操作 装配 线 
中 ,倒置 安装 是 一 个 应 用 。 
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图 12-19 安装 在 天 花 板 上 的 2R 机 械 手 





1 $ 1 
=K —V=-; mibi tymi HZ3 CO1 +02) ?+21112(01+02)cos02)+ 


miglicos01 +m2g (lı cos@; +lz2cos(01ı 十 02 ) ) 
(12-420) 
式 (12-420) 会 产生 下 列 的 运动 方程 。 
Qi =O +m) +m2l3 +2m21 11200802 16 ı +m2zl2(l2+l1ıcosh2)Ö 2— 





2m21112sinO20 16 2—mol,l2sinb263+(mi +m.) gli 
sind; +m2 gl2sin(@; 十 02 ) 
(12-421) 
Qo =mel2(L2 +11 cosb2) 6 1 +m213 6 2 —2my1, 12 sind26 1 (61 +62)—moglosin(O; +02) 
(12-422) 
运动 方程 可 调整 为 
Q1=Di 1 +D 2+Hind{ +H ix 
0 





24D 1126102+D12620;+G, (12-423) 

















Qo =Diz 6 1 +Do26 9+ Ho 04 +H 2020 3 +D20126162+D218201+G2 (12-424) 














这 里 
Di =(mı +m2 Li +m213 +2m2 1112 cosb2 (12-425) 
Diz =m?2l2 (l2 +11 cos2 ) (12-426) 
D2 =Di2 =moel2 U2 +11 cosb2) (12-427) 
Do2=m213 (12-428) 
Hin=0 (12-429) 
H 122 =— mel, 12 sind2 (12-430) 
H21 =— mel, l2sind2 (12-431) 
H 222 =0 (12-432) 
H n2 = Hin =—melilesinds (12-433) 
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H 212 = H 221 = — m2 lı l2 sinĝ2 (12-434) 
Gi =m +m2) gli sind; +m2 gl2sin(@1 +02) (12-435) 
G2 =m glzsin(01 +82) (12-436) 





例 12-22 具有 质量 连 杆 的 2R 机 械 手 。 
具有 质量 连 杆 的 2R 平面 机 械 手 如 图 12-20 所 示 。 我 们 假设 每 根 连 杆 的 质心 C 位 于 连 杆 
的 中 间 位 置 ， 关 节 处 的 电动 机 是 无 质量 的 。 连 杆 的 变换 和 矩阵 为 























x) Vy 











Hh 





图 12-20 具有 质量 连 杆 的 2R FOL BRA 




















cosO1 一 Sin01 0 LicosOl 
sind, coOSO1 0 isin01 
T= (12-437) 
0 0 1 0 
0 0 0 1 
cos0» 一 Sin0。， 0 12cosO， 
sing COSO 0 lsin 
Ty =| ? A (12-438) 
0 0 1 0 
0 0 0 1 





cos(0ı 02) sin(@; +62) 0 Licos01 十 12cos(CO1 +02) 
sin(O1 十 02)  cos(@:+62) 0 0sin01 十 02sin(CO1 +02) 
0 0 1 0 
0 0 0 1 








OT =9T,!T2 = 


(12-439) 


XY WER I. MARERE Ar， 我 们 可 以 写 出 : 





0 一 1 0 O)fcos#; 一 Sin01 0 LicosOl 





YT, 1 0 0 0 1 cos; 0 asin0 
A =A,°T, = 
wae 0 0 0 0 0 0 1 0 

0 0 0 0 0 0 0 1 
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一 Sin01 一 cos0O 0 一 /isin0l 
cosO1 ”一 Sin01 0 LacosOl 
= (12-440) 
0 0 0 0 
0 0 0 0 
类 似 地 ， 有 
0 一 1 0 0)/cos(010,) sin(01 +42 ) lı cosfı 十 /2 cos(O1 +62) 
IT? 1 0 0 O}} sin(@; +02) cos(CO1 +62) lı sinfı 十 /2Ssin(CO1 +62) 
一 ARoT， 一 
901 0 0 0 0 0 0 if 0 
0 0 0 0 0 0 0 1 
sin(01 +02) cos(O1 +02) O 一 sin0l 一 02Sin(O +02) 
cos(@; +02) 一 Sin(O1 +62) 0 Licos0l 十 /2cos(CO1 +42) 
0 0 0 0 
0 0 0 0 
(12-441) 
sin(@, +02) cos(0ı +02) 0 一 /2sin(CO1 +82) 
PT, cos(0ı +02) —sin(0ı +02) 0  Llzcos(0ı +082) 
=T AR! T: = 
302 0 0 0 0 
0 0 0 0 
(12-442) 
假设 所 有 惯性 积 均 为 0， 我 们 求 得 
E a 1 
miei o 0 =mil 
= 0 0 0 0 
1T1 一 (12-443) 
0 0 0 0 
二 oa 
g 1 mı 
= Ey O Ü D l 
本 7242 z m2l? 
= 0 0 0 0 
2 了 ;> 一 (12-444) 
0 0 0 0 
ails 0-6 
g me 2 m2 
PEKERE UEHARA AT LICH A PE D). 
oT, — IT OT, — ƏT 
Dn=e 人 2 Thi I ! JH l 227 I, ” |] 
Iq 991 9g1 9g1 (12-445) 
1 
一 一 77212713 Hmi | 二 上 7 221722 coSO> 
3 3 
YT, — aT T 1 
Dj.2=D2,=tr 23 2 2 二 m21? 十 二 m113 十 m2l1l2cos0 (12-446) 
9g1 9g1 3 
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OT — 3l TT 1 
Da=t( 22T, — ) mo2l3 (12-447) 
dq? dq? 3 
HAM Acq, g) 的 计算 如 下 
2 2 
Hı = >) >) A tind edm= Hindi dit Hndidst HinG291+ Hizzġzġ2 (12-448) 
k=lm=1 
2 2 
Hz = >) >) Hmdrdn= Handi dit Hazdidst Hod2dit+ H2224242 (12-449) 
k=l1m=1 
这 里 
iG 32T, _ aoT,T 
Hix= >) "| "I, } (12-450) 
r=maxi,j +k Iq; 9k Iqi 
这 些 计算 会 导致 
1 Sun e 
Top melil20 sind, ~ m2l1128 18 zsinz 
H= (12-451) 
ym 2lil20 {sinz 
最 后 项 是 重力 加 速度 矢量 G (gq) 
DT; aV To 
一 T 1 T 29 
G1 mı g Igi rl m2 g aqi r2 
0 \'(—sinOd; 一 cos01 0 —l sind; (一 0172 
一 站 coSsO1 一 Sin01 0  licost; 0 
= mti — 
0 0 0 0 0 
0 0 0 0 0 1 
0 \'(—sinOi2 —cosOi2 0 一 /isin01l 一 /2Ssin0ly (一 0172 
=p cos6 12 —sindiz 0  Zıcosĝı +lecosbi2 0 
m2 
0 0 0 0 0 0 
0 0 0 0 0 1 
1 
=z migli cosi tmz gl cos(01 +02) +m2glı cost, 
(12-452) 
0 \T(—sin@i2 —cos@i2 0 一 /2Ssin0ly (一 0172 
PT, 一 将 cosĝ12 —sinOj2 0  ļ2cos0ı2 0 
G? =~ m2 gi 272 一 一 702 
ðq? 0 0 0 0 0 0 
0 0 0 0 1 
1 
= Fm2g8l2cost 0 +02) 
(12-453) 


最 后 ，2R 平面 机 械 手 的 运动 方程 为 
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1 1 
mili tme| 1? +a—l3+lilscos2 | m|? +> m +Z l2 cosô2 j 
Qı 3 3 A 
Qa) 7 
vA 


7 L 1 
ma (1+4 +h Lacoste | znel? 


— m2l1l20 3 sinz —m2l1l20102sing; 
十 


Fm2l1l20 Tsing 


1 
5 mzglıcos(0ı +02) Fm2 gli cost) 


mı glicos0; + 2 


1 
2 


1 
z 72gl2cos(ĝı +02) 





(12-454) 


12.5 机 器 人 静 力 学 


在 从 静止 到 静止 Crest-to-rest) 任务 的 开始 和 结束 时 ， 机 器 人 必须 保持 既定 的 配置 。 为 
了 保持 位 置 和 方向 ， 执 行 器 必须 施加 一 些 所 要 求 的 力 以 平衡 机 器 人 受到 的 外 部 载荷 。 计 算 所 
要 求 的 执行 力 以 保证 机 器 人 的 具体 配置 ， 这 称 为 机 器 人 苦力 学 分 析 。 

在 静态 条 件 下 ， 连 杆 G) 在 全 局 坐标 系 中 表述 的 牛顿 欧 拉 方程 可 以 递归 形式 写 为 











OF, 1=°F;— °F. (12-455) 
°Mi-1=9Mi — X°? Ma +; di X °F: (12-456) 

这 里 
di 一 0d; 一 0d (12-457) 


因此 ， 当 反作用 力 系统 (一 F;， 一 M;) 给 定时 我 们 能 够 计算 施 力 系统 (F;-1，M;-_1)。 
连 杆 (i) 上 的 位 置 拓 量 和 力学 系统 如 图 12-21 所 示 。 














iE AB : 

在 静态 情况 下 ， 连 杆 G) 牛顿 - 欧 拉 运动 方程 [ 式 (12-1) MR (12-2) ] 分 解 成 力 和 力 
EFM TE. 
F; F;+>° F.; =0 (12-458) 
°M;-1 °M: + >°Me +n; X°F;—-1—°m; X? F; =0 (12-459) 
这 些 方程 可 以 被 调整 成 一 个 逆向 递归 形式 。 

Fi- 5? Fi—2° Fe (12-460) 
Mi- 5M: — XMa —n; X °F ;-1 +m; XF; (12-461) 





然而 ， 我 们 可 以 转换 从 质心 C; 到 O;-1 的 欧 拉 方程 ， 并 且 求 解 式 (12-456). 
实际 上 ， 我 们 可 以 测量 附着 在 连 杆 G) 上 的 坐标 系 B: 中 B: 和 B;-1 的 相对 位 置 和 质心 
的 位 置 撩 量 r;。 因 此 ， 我 们 必须 将 矢量 'r; 和 ; ?4d; 转 换 到 基体 坐标 系 中 。 
0r; =°T;' r; (12-462) 
fd; =°T;; d; (12-463) 
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关节 计 1 
(反作用 关节 ) 








0i 


图 12-21 连 杆 G) 上 的 位 置 矢 量 和 力学 系统 





外 部 载荷 通常 是 重力 Mig» 因此 ， 有 








X F.i=m;° g (12-464) 
XMa = ri Xmi°g (12-465) 
利用 D-H 参数 ， 我 们 可 以 用 下 式 表述 相对 位 置 矢量 ,_1 4d;。 
Qi 
: disina; 
iid; = (12-466) 
dicosa; 
1 


向 后 递归 方程 [ 式 (12-455) 和 式 (12-456) ] 允许 我 们 从 一 个 在 B 处 已 知 的 力学 系统 
(ŒF. Mi) 开始 。 该 力学 系统 从 末端 执行 器 作用 于 环境 ， 在 B,-1 处 计算 力学 系统 。 
Fp =? Fn Fon (12-467) 
0M，1 一 0M — Men +, idn, X? F, (12-468) 
按照 相同 的 过 程 ， 通 过 每 根 连 杆 远 端 处 的 力学 系统 计算 近 端 处 的 力学 系统 ， 在 基体 处 结 
束 力学 系统 。 在 这 个 过 程 中 ,假设 末端 执行 器 作用 于 环境 的 力学 系统 已 知 。 
重新 将 式 (12-458) 和 式 (12-459) 调整 为 正 向 递归 形式 ， 这 也 是 有 可 能 的 。 


OF; =9 Fia HY? Fe (12-469) 

0M; =9M;-1+>°Mei +n: XIF; 1—m; X? F; (12-470) 
转换 从 质心 C: 到 O; 的 欧 拉 方 程 可 以 将 正 向 递归 方程 简化 为 更 加 实际 的 方程 。 

OF, =9 Fi + Fe (12-471) 

0M; =°Mi-1+ 2 °Mea— ;-1di X? Fi (12-472) 





利用 正 向 递归 方程 [ 式 (12-471) MÈ (12-472)]， 我们 可 以 从 在 坐标 系 Bo 处 的 已 知 
力学 系统 (Fo. Mo) 开始 ， 该 力学 系统 应 用 于 连 杆 (1)， 然后 计算 坐标 系 B 处 的 力学 








OF, = Fo +> Fa (12-473) 
"Mı =Mo+ XMa — di X Fo (12-474) 
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按照 这 个 过 程 ， 通 过 每 根 连 杆 近 端 力学 系统 计算 远 端 力学 系统 ， 结 束 于 末端 执行 器 应 用 
于 环境 的 力学 系统 。 在 这 个 过 程 中 ， 假 设 基体 执行 器 施加 于 第 1 根 连 杆 的 力学 系统 已 知 。 

例 12-23 4R 平面 机 械 手 的 静 力 学 。 

图 12-22 所 示 为 一 个 为 每 根 连 杆 建立 D-H 坐标 的 AR 平面 机 械 手 。 假 设 被 测 的 应 用 于 环 
境 的 末端 执行 器 力学 系统 为 

















图 12-22 为 每 根 连 杆 建立 D-H 坐标 的 AR 平面 机 械 手 








F 0 
4 一 |F ,| M=] 0 (12-475) 
0 M: 
除 此 之 外 ， 我 们 假设 连 杆 是 均匀 的 ， 这 样 一 来 其 质心 C 便 位 于 每 根 连 杆 的 中 点 处 ， 并 
且 重 力 加 速度 为 
8 一 一 sj (12-476) 








机 械 手 由 四 根 R || R(Co)7 连 杆 构 成 ， 因 此 它们 的 变换 矩阵 一 :Ti; 是 式 (5-32) 的 类 型 。 
因为 di 50 和 a; 二 1;， 因 此 其 变换 矩阵 可 简化 为 





cos0; 一 Sin0; 0 l;cosd; 
sind ; cosd; 0 J;sind; 
i“1T;,= (12-477) 
0 0 1 0 
0 0 0 I 
质心 C MAK Er: 和 相对 位 置 矢量 ;7d ,为 
Li /2 b 
0 0 
r= id: = (12-478) 
0 0 
0 0 
因此 ， 有 
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0r; 一 0T r; (12-479) 
;-$d;=°T;,_id; (12-480) 
这 里 
"T; =° Ti1: : IT; (12-481) 
关节 3、2、1 处 的 静态 力 为 
Fz 0 Fz 
A F, 1 Fy+mag 
OF; =° Fi X? Fa = Fi tmig®jo= : +mig ale i (12-482) 
0 0 0 
Fa 0 F, 
z Fy+m Fy 十 (ms 二 ma)g 
°F, =° F; +m3g?jo = : ‘ i m3 2 三 | Gg (12-483) 
0 0 
Fz 0 Fz 
š Fy+(m3+m4)¢ 1 Fy+(m2+m3+m4)g 
OF, =F +mg? jo = y 3 478 izg = a 2 3 478 
0 0 0 
0 0 0 
(12-484) 
Fz 0 Fz 
Fo =° Fi tmig?jo= Fy+(m2+m3+m,)g mE 1 F, +m +m: +m; +m,)g 
0 0 0 
0 0 0 
(12-485) 
关节 3、2、1 处 的 静态 力矩 为 
°M;=°M, > Mei | 3 dy X° Fı =°M, +’rı Xmag? jo +? di x°R, 
0 
0 (12-486) 
="MitmigOT,! r4 KF HCT: t d4) X? 五 4 一 M 
0 
1 
M32 =M. +L4F y cos0 1234 — l4 Fx sinO 1234 ty slams cosh 234 (12-487) 
0 
a 0 
°M: =°M; +m;g Ta rs KF IHCT33d3) X? F; = M (12-488) 
2z 
0 
1 
M2: =M: FLF ycos@izs1 — la F x sind 1234 + glam cosdizs4 + 
1 
g slamscosizs ~l3 Fx sinOiz3 Hla (Fy Emag )cosdizs (12-489) 
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0 
"Mı =°M: +mzg CT2° r2 KF IHKOTs? d,) X°F.= M (12-490) 
lz 


0 


Miz =M, 十 /4 F, cosĝ1234 — l4 Fy sin 1234 十 Selim, costs t 





Selsms costs —ls Fa sind +13 (Fy +mag)cos0123 + let) 
1 : 
9 Bl2m2zcos6 12 — La F a sinf12 tloL Fy +m; +m1)g JcosO1 
0 
oOMo=° Mitmig CTi! r1X? fo) HOT} di) X° Fi = Pa (12-492) 
0 


Moz =M, +1, Fy cosd i234 — la F. sin 1234 十 Salam, cost25 + Fglamacosbis — 


la F+ sinĝ123 +13 (Fy +m. g)cos0i23 +> > elim coy, lF, sin012 十 





1 
8l mı cosh —1,F,sin@,; + 


LLE, +m: +m; +m,)g jcosOl 


LLIF, (m3 ma)g jcosO12 十 











(12-493) 
这 里 
01234 =01 +02 +03 +04 (12-494) 
0123 =01 +02 +0; (12-495) 
012 =01 +02 (12-496) 





例 12-24 连 杆 坐标 系 中 的 递归 力学 方程 。 
实际 上 ， 比 较 容 易 测 量 和 计算 连 杆 坐 标 系 中 的 力学 系统 。 因 此 ， 我 们 可 以 下 列 形 式 写 出 
逆向 递归 方程 : 





iF; 1 一 :下 ;十 2 下。 (12-497) 

iM;_1=iM;— Mt, 1d; X'F; (12-498) 

计算 连 杆 坐标 系 中 距 远 端 力 学 系统 的 近 端 力学 系统 。 这 时 被 计算 的 力学 系统 可 以 通过 变 
矩阵 换 被 转换 到 前 一 连 杆 坐标 系 中 。 








i—1 F; =T; F; (12-499) 
i—1M;-1 =i T; `M; (12-500) 

或 者 转换 到 任何 其 他 坐标 系 中 ， 包 括 基体 坐标 系 。 
oF, 1=°T,iF; i (12-501) 
oOM; 1=°T,iM;_ i (12-502) 
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例 12-25 ”执行 器 的 力 和 力矩 。 

应 用 正 向 或 逆向 递归 静 力 学 分 析 通 常 以 位 于 关节 处 的 一 组 已 知 力学 系统 而 结束 。 每 个 关 
节 由 一 个 电动 机 驱动 ， 或 者 通常 由 施加 一 个 力 于 了 关节 的 一 个 执行 器 驱动 ， 或 者 由 位 于 及 
关节 (旋转 关节 ) 处 的 一 个 转 矩 驱动 。 当 关节 ; 是 平 动 关节 ， 执 行 器 的 力 将 沿 着 关节 ; 的 轴 
线 施加 。 因 此 ， 了 驱动 电动 机 的 力 是 沿 着 z;-1 轴 的 。 

下 ,一 "ET oF; (12-503) 
st (12-503) DAH, SAF We; RHR. AMAR 7-1 A fi RB 
分 由 关节 轴承 确定 。 

类 似 地 ， 当 关节 i 是 旋转 关节 时 ， 执 行 器 转 矩 是 绕 着 关节 i 的 轴线 的 。 因 此 ， 了 驱动 电动 
机 的 转 矩 是 沿 着 x;-1 轴 的 。 






































Mn =° M; (12-504) 
式 (12-504) 说 明 ， 关 节 力 矩 M; 的 Ri-; 部 分 由 执行 器 确定 ， 然 而 关节 力矩 M; h ia M 1 
的 部 分 由 关节 轴承 确定 。 


12.6 本章 小 结 





机 器 人 的 运动 动力 学 方程 可 以 由 牛顿 - 欧 拉 法 和 拉 格 于 日 法 求 得 。 因 此 ， 在 牛顿 - 欧 拉 法 
中 每 根 连 杆 (i) 是 一 个 刚体 ， 在 基体 坐标 系 中 其 平 动 和 转动 的 运动 方程 为 








mi? ai= Fii Fit? Fo (12-505) 
ee æ; =° Mi- M; +X? Ma +O da ri)X? Fi C di—? ri)X? F; 
(12-506) 





力 F;-1 和 力矩 M;-1 是 连 杆 (i 一 1) 在 关节 i 处 作用 于 连 杆 (i) 的 合力 和 合力 矩 。 类 似 
地 ，F; 和 M; 是 连 杆 G) 在 关节 i 十 1 处 作用 于 连 杆 (i 十 1) 的 合力 和 合力 矩 。 我 们 分 别 在 
坐标 系 Bii MERR B: 的 原点 处 测量 力学 系统 (Fi;-1，M;-1) M (Fi, M:i), SKRBE T 
作用 于 连 杆 G) 的 合力 和 合力 矩 是 Fe 和 2M.。;。 矢 量 和 r; 是 质心 C; 的 全 局 位 置 矢 量 ，"d， 
是 坐标 系 B; 原点 的 全 局 位 置 矢 量 。 矢 量 "e; 是 角 加 速度 ,"a; 是 连 杆 G) 在 质心 C; 处 所 测 的 
平 动 加 速度 。 
































oa; =od ito ai XC ri —° dito @: X[o oi XO Pr 一 "di)] (12-507) 


a 0@:-1 +8 ;0k; 1 十 。 @; XO ki 1 (对 于 R 关节) (12-508) 
0@i-1 (对 于 了 关节 ) 
重力 通常 是 位 于 机 器 人 的 中 间 环 节 的 唯一 外 部 载荷 ， 来 自 环境 的 反作用 力 是 作用 于 基体 
和 末端 执行 器 连 杆 上 的 附加 外 部 力学 系统 。 基 体 执行 器 施加 于 第 1 根 连 杆 上 的 力 和 力矩 是 
Fo。 和 Mo， 末 端 执行 器 应 用 于 环境 的 力 和 力矩 是 F, MM, 。 如 果 重 力 连 杆 (i) 上 的 唯一 外 部 
载荷 ， 并 且 沿 着 一 "Re 方向 ， 这 时 有 
°F =m: g=—migko (12-509) 
Mes =r; Xm) g =r; Xmigrko (12-510) 
这 里 ，g 是 重力 加 速度 矢量 。 
牛顿 - 欧 拉 运动 方程 也 可 以 用 正 向 或 者 逆向 法 在 连 杆 坐标 系 中 书写 。 连 杆 (i) 在 局 部 坐 
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标 系 B: 中 的 逆向 牛顿 - 欧 拉 运动 方程 为 





IF; 5i F Xi Fetma; (12-511) 

iMi =M: — Xi Ma — C di-1— ir) Xi Fi-1 + CG di —i ri) X 
了 十 Ti 二 io X io; (12-512) 

这 里 

in; =' d;—1—Ż Tri (12-513) 
im; =d; — ir; (12-514) 
bai =i Ë; tia: XCri—' d+), o: X h @;XCr,—id;)] (12-515) 
ia: = Ti G aii H kD HT C do X k) (对 于 及 关节 ) 
B ‘Tia’ 7 Oi (对 于 了 关节) 
(12-516) 


在 这 种 方法 中 ， 我 们 通过 已 知 被 驱动 力 系统 CF: Mi) 和 合成 外 力 系统 CFei+'Mei) 
寻找 驱动 力 系统 CFi-1.'Mi-1). SEMA B: 中 求解 驱动 力 系统 CFi-1.'Mi-1) 时 ， 
我 们 可 以 将 其 转换 到 坐标 系 B;-1 之 中 ， 并且 对 连 杆 (i 一 1) 应 用 牛顿 - 欧 拉 方程 。 


il F;—1 =i IT iF, i (12-517) 





IMi-1=' 'Ti'Mi-1 (12-518) 
对 于 连 杆 (i 一 1) 来 说 ， 转 换 力 学 系统 的 负 号 表示 被 驱动 力 系统 (一 全 1Fi-1,， 一 1M;-1)。 
EIF OO 在 局 部 坐标 系 B: 中 的 正 向 牛顿 - 欧 拉 运动 方程 为 





下; =i F; HXi Fa mij ai (12-519) 
iM: 一 Mi HXi Ma HC dai? ri) Xi Fi C diiri) X 
'F;—'1;,a@:;—}@; X'1;),0; (12-520) 
in; =' di-1—ir; (12-521) 
im; ='d;—'r; (12-522) 


利用 正 向 牛顿 - 欧 拉 运动 方程 ， 我们 可 以 通过 已 知 的 作用 力 系 统 CF i-1.'Mi-1) 计算 
反作用 力 系 统 (iF;,'M;)。 当 在 坐标 系 B 中 求 得 反作用 力 系统 CF: M:i) 时 ， 我 们 可 以 
将 其 转换 至 坐标 系 Bii Pe 











itp, = TF, (12-523) 
MT 一 上 TD M; (12-524) 


对 于 连 杆 (i 十 1)， 被 转换 力学 系统 的 负 号 表示 作用 力 系 统 (一 个 1F;， 一 站 1M;), 我 
们 可 以 对 连 杆 (i 十 1) 应 用 牛顿 - 欧 拉 方程 。 正 向 牛顿 - 欧 拉 运动 方程 允许 我 们 从 一 个 已 知 作 
FAAS CFo.'Mo) 开始 ， 基 体 连 杆 作用 于 连 杆 (1)， 然 后 再 计算 下 一 根 连 杆 的 作用 力 。 
因此 ， 一 个 一 个 地 分 析 机 器 人 连 杆 ， 我 们 以 末端 执行 器 作用 于 环境 的 力学 系统 而 结 

拉 格 天 日 运动 方程 为 























ae = Q L=1,2 (12-525) 
dt agi daqi i 1 sls n 
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f=K—V (12-526) 
式 (12-526) 提供 了 获得 机 器 人 动态 方程 的 一 个 系统 方法 。 变 量 g; 是 表述 能 量 的 坐标 ， 
Q; 是 相应 的 常规 广义 力 。 
对 于 nn 根 串 联 连 杆 的 机 械 手 ， 其 基于 牛顿 - 欧 拉 或 者 拉 格 朗 日 运动 方程 总 是 可 以 矩阵 形 
式 进行 设置 。 
Dg)g Hq. ÐG) =Q (12-527) 
或 者 
Di@q+Clq: gt+G(q)=aQ (12-528) 
或 者 以 求 和 的 形式 表示 
XD, Dä; F 5) Siae G: =Q; (12-529) 
k=l1m=1 
XE, Dq) 是 一 个 ts Tat HEMT EKHE RE. 
D= 5 [Bim id ov + Sd ELT | (12-530) 
五 是 速度 耦合 矢量 
Has GA i (12-531) 
j=lk=l dqr 2 ðqi 
G; 是 重力 矢量 
ee > mj gtd} (12-532) 
Ji 是 机 器 人 雅 可 比 矩 阵 。 
。 人 Oe ; 
六 ;一 = q=J iq (12-533) 
O; J Ri 
AT (ELAS AAD FAS BC. PAT a Zi i A — 2 fr BE OR A WA ot oF PL AY SP 
力 。 在 静态 状况 下 ， 连 杆 G) 表述 在 全 局 坐标 系 中 的 牛顿 - 欧 拉 方 程 可 以 递归 形式 写 出 : 
oF; = F;— F (12-534) 
OM: =°M;: —X°Ma +; Yd; X°F; (12-535) 
现在 ， 当 反作用 力 系 统 ( 一 F;， 一 M;) 给 定时 ， 我 们 能 够 计算 作用 力 系 统 (F;-1,M;-1)。 
习题 
12-1 标记 与 符号 。 
描述 下 列 这 se 
1) F: 2) 9% 3) 1F; 4) °M; 5) ?Ma 6) BM 
7) m? 8) w 9) oid; 10) "d 11) °r; 12) ld; 
13) °L; 14) "7T， 15) ;& L; 16) K; 17) V; 18) "71I; 
K12-2 ”偶数 阶 递归 平移 速度 。 
试 求 将 连 杆 G) 速度 与 连 杆 (i 一 1) ERE, EI (i 十 2) 速度 联系 起 来 的 方程 。 
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We12-3 偶数 阶 递归 和 角速度。 

试 求 将 连 杆 (i) 角速度 与 连 杆 (i 一 1) 角速度 、 连 杆 (i 十 2) 角速度 联系 起 来 的 方程 。 

友 12-4 偶数 阶 递归 平移 加 速度 。 

试 求 将 连 杆 (i) 加 速度 与 连 杆 (i 一 2) 加 速度 、 连 杆 (i 十 2) 加 速度 联系 起 来 的 方程 。 

Wel 2-5 偶数 阶 递归 角 加 速度 。 

BOR KIER G) 角 加 速度 与 连 杆 (i 一 2) 角 加 速度 、 连 杆 G+2) 角 加 速度 联系 起 来 
的 方程 。 

w12-6 不 同 坐标 系 中 的 加 速度 。 

对 于 图 12-7 所 示 的 2R 平面 机 械 手 ， 试 求 yJay laz, Sai, bai, ba: Mia. 

12-7 曲柄 滑 块 机 械 动力 学 。 

如 图 12-23 所 示 的 一 个 平面 曲柄 滑 块 机 构 。 建 立 连 杆 坐标 系 ， 推 导 牛 顿 - 欧 拉 运动 方程 ， 
试 求 基体 旋转 关节 处 的 传动 力矩 。 

12-8 PR 机 械 手 动力 学 。 

对 于 如 图 5-56 所 示 的 平面 极 坐标 机 械 手 ， 试 求 其 运动 方程 。 消 除 关 节约 束 力 和 关节 约 
束 力矩 ， 推 导 执 行 器 的 驱动 力 或 驱动 力矩 的 方程 。 

12-9 平面 笛 卡 儿 机 械 手 。 

确定 如 图 12-24 所 示 平 面 笛 卡 儿 机 械 手 的 运动 方程 。 提 示 : 坐标 系 并 不 是 基于 D-H 规 
则 的 。 
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图 12-23 平面 曲柄 滑 块 机 构 图 12-24 2 自由 度 笛 卡 儿 机 械 手 


友 12-10” 连 杆 动量 的 全 局 微分 。 
在 递归 牛顿 - 欧 拉 运动 方程 ， 为 什么 我 们 不 使 用 下 列 的 牛顿 方程 呢 ? 
ye ip Ss v =mi ó +) @;Xmiv 
dt dt 
12-11 3R 平 面 机 械 手 动力 学 。 
一 个 3R 平面 机 械 手 如 图 12-29 所 示 。 机 械 手 被 附加 安装 到 墙 上 ， 因 此 g 一 g 人 70。 
1) 试 求 机 械 手 的 牛顿 - 欧 拉 运动 方程 。 在 全 局 坐标 系 中 进行 计算 ,推导 每 个 关节 处 的 驱 
动力 和 驱动 力矩 。 
2) 对 于 关节 和 矩 ， 将 方程 数目 减少 到 3 个 。 
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3) 替换 矢量 值 ， 依 据 机 械 手 的 几何 变量 和 角 变 量 计算 力矩 。 

12-12 平面 笛 卡 儿 机 械 手 动力 学 。 

确定 如 图 5-57 所 示 平 面 笛 卡 儿 机 械 手 的 牛顿 - 欧 拉 运 动 方程 。 

12-13 ”关节 式 机 械 手 。 

图 12-25 所 示 为 一 个 具有 无 质量 机 械 臂 和 两 个 质点 mi 和 m2 的 关节 式 机 械 手 。 














图 12-25 关节 式 机 械 手 





1) 按照 D-H 规则 建立 连 杆 坐标 系 。 

2) 确定 D-H 变换 矩阵 。 

3) 利用 拉 格 朗 日 法 确定 机 械 手 的 运动 方程 。 

12-14 极 坐 标 平面 机 械 手 动力 学 。 

2 自由 度 极 坐标 平面 机 械 手 如 图 5-56 所 示 。 

1) 确定 机 械 手 的 牛顿 - 欧 拉 运动 方程 。 

2) 对 于 基体 关节 处 的 力矩 和 PP 关节 处 的 力 ， 将 方程 数目 简化 为 两 个 方程 。 

3) 替换 矢量 值 ， 依 据 机 械 手 的 几何 变量 和 角 变 量 计算 作用 力 和 作用 力矩 。 

W12-15 球形 机 械 手 动力 学 。 

图 5-43 所 示 为 一 个 附带 有 球形 手腕 
的 球形 机 械 手 。 分 析 该 机 嚣 人， 推导 关 
节 作用 力 和 关节 作用 力矩 的 运动 方程 。 
假设 g == 一 g"ko。， 末 端 执行 器 承载 着 一 
质量 m, 

W12-16 关节 式 机 械 手 动力 学 。 

图 5-22 所 示 为 一 个 R 上 RR 关节 
式 机 械 手 。 利 用 g =— ho 求 得 机 械 手 
运动 方程 。 

12-17 平面 机 械 手 。 

图 12-26 所 示 为 一 个 3 自由 度 平 面 
机 械 手 。 如 果 连 杆 是 无 质量 的 并 且 有 两 
VR m 和 m2>， 那 么 请 确定 该 机 械 手 





















































图 12-26 3 自由 度 平面 机 械 手 
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的 运动 方程 。 

%& 12-18 SCARA 机 器 人 动力 学 。 

如 果 g 王 一 5"&o， 那 么 请 计算 图 5-23 所 示 R || RI RI P KWAI SCARA 机 器 人 的 动态 
关节 力 系 统 。 

% 12-19 SRMS 机 械 手动 力学 。 

图 5-24 所 示 为 航天 飞机 远程 操作 机 械 手 系统 SRMS 模型 。 

D 对 于 g 二 0， 推 导 SRMS 的 运动 方程 并 且 计 算 关 节 力 系统 。 

2) 对 于 g 王 一 50&u， 推 导 SRMS 的 运动 方程 并 且 计 算 关节 力 系 统 。 

3) 消除 约束 力 ， 减 少 方程 数目 使 其 等 于 作用 力矩 的 数目 。 

4) 假设 连 杆 是 由 半径 rv = 0. 25m 和 线 密 度 m=12kg/m 的 均匀 圆柱 构成 。 利 用 表 5-11 
中 所 给 出 的 特征 求 得 运动 方程 ， 当 末端 执行 器 一 直 夹 持 着 一 个 24kg 的 质量 块 。 

12-20 3R 平面 机 械 手 递归 动力 学 。 

图 12-29 所 示 的 机 械 手 是 安装 在 墙壁 上 的 一 个 3R 机 械 手 ， 因 此 g 一 一 50zo。 

D 利用 逆向 递归 牛顿 - 欧 拉 技术 试 求 机 械 手 运动 方程 。 

He 2) 利用 正 向 递归 牛顿 - 欧 拉 技术 试 求 机 械 手 运动 方程 。 

12-21 RPR 平面 兄 余 机 械 手 。 

图 12-27 所 示 为 具有 关节 变量 0 dz: M0: 的 一 个 3 自由 度 平面 机 械 手 。 如 果 连 杆 是 无 
质量 的 并 且 有 两 个 质点 mi 和 mr， ， 请 确定 该 机 械 手 的 运动 方程 。 






















































































图 12-27 RPR ICAL ARE 








12-22 极 坐标 平面 机 械 手 递归 动力 学 。 

图 5-56 所 示 为 一 个 2 自由 度 极 坐 标 平面 机 械 手 。 

1) 利用 逆向 递归 牛顿 - 欧 拉 技术 试 求 机 械 手 运动 方程 。 

He 2) 利用 正 向 递归 牛顿 - 欧 拉 技 术 试 求 机 械 手 运动 方程 。 

W12-23 关节 式 机 械 手 递归 动力 学 。 

图 5-22 所 示 为 一 个 R 上 FRR 关节 式 机 械 手 。 利 用 g 二 一 g"&k。 和 下 列 技术 求 取 机 械 手 的 
运动 方程 。 

1) 利用 逆向 递归 牛顿 - 欧 拉 技术 。 
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2) 利用 正 向 递归 牛顿 欧 拉 技术 。 
% 12-24 SCARA 机 器 人 递归 动力 学 。 
RIRIRIP 关 节 的 SCARA 机 器 人 如 图 5-23 所 示 。 如 果 g 王 一 50&o， 请 通过 下 列 技术 
确定 动力 学 运动 方程 。 
D 利用 逆向 递归 牛顿 - 欧 拉 技术 。 
2) 利用 正 向 递归 牛顿 - 欧 拉 技术 。 
友 12-25 SRMS 机 械 手 递归 动力 学 。 
图 5-24 所 示 为 航天 飞机 远程 机 械 手 系统 (SRMS) 的 模型 。 
1) 对 于 g 王 0， 利 用 逆向 递归 牛顿 - 欧 拉 技术 推导 SRMS 的 运动 方程 。 
2) 对 于 g 二 0， 利 用 正 向 递归 和 牛顿- 欧 拉 技 术 推 导 SRMS 的 运动 方程 。 
12-26 3R 平面 机 械 手 的 拉 格 朗 日 动力 学 。 
利用 拉 格 朗 日 技术 试 求 如 图 12-29 所 示 的 3R 平面 机 械 手 运动 方程 。 机 械 手 安装 到 一 堵 
上 ， 因 此 g 王 一 g&"zo 。 
12-27 极 坐 标 平面 机 械 手 拉 格 朗 日 动力 学 。 
利用 拉 格 明日 技术 试 求 如 图 5-56 所 示 的 极 坐标 平面 机 械 手 运动 方程 。 
W12-28 关节 式 机 械 手 拉 格 朗 日 动力 学 。 
图 5-22 所 示 为 一 个 RF 上 FRR 关节 式 机 械 手 。 利 用 g =— g ko 和 拉 格 朗 日 技术 求 取 机 械 
手 运 动 方 程 。 
友 12-29 SCARA HLA A Fite BA A HHS. 
RI RI RI P Æi SCARA 机 器 人 如 图 5-23 所 示 。 如 果 g 王 一 go&o， 那 么 应 用 拉 格 衣 
日 技术 确定 该 机 械 手 的 运动 动力 学 方程 。 
% 12-30 SRMS 机 械 手 的 拉 格 朗 日 动力 学 。 
图 5-24 所 示 为 航天 飞机 远程 机 械 手 系统 (SRMS) 模型 。 对 于 下 列 的 重力 加 速度 ， 利 
用 拉 格 朗 日 技术 确定 SRMS 的 运动 方程 。 
1) g=0, 
2) g=—g°ko. 
W12-31 执行 器 所 做 的 功 。 
考虑 在 一 个 给 定 路 径 上 移动 的 2R 平面 机 械 手 。 假 设 2R 机 械 手 的 端点 以 定常 速度 v= 
lm/s 在 由 两 个 半圆 组 成 的 路 径 上 从 Pi 移动 到 Po, WE 13-35 所 示 。 如 果 1) =l2=1m A 
机 械 手 正 承载 着 12kg 的 质量 ， 那 么 计算 由 驱动 器 所 做 的 功 。 该 圆 的 圆心 位 于 〈0. 75m, 
0.5m) 和 (—0.75m, 0.5m). 
12-32 2R 平面 机 械 手 静 力 学 。 
图 12-28 所 示 为 一 个 安装 到 天 花 板 上 的 2R 平面 机 械 手 。 均 匀 连 杆 的 参数 如 下 : 
m,=24kg, mz2=18kg, lı=lm, 





























ae 


















































l2=1m, g= g’ joo 
在 端点 处 有 一 载荷 及 .= 一 14g01 。 分 别 计算 0; =30" 和 0 一 45" 时 的 静 力 矩 O; 和 Qs。 
12-33 在 不 同 底 角 处 2R 平面 机 械 手 的 静态 学 。 
在 习题 12-32 中 ， 保 持 9, = 45°. 计算 作为 01 的 函数 的 静 力 矩 Q1 和 Qs,。 绘制 Q1 MQ, 
对 01 的 曲线 ， 求 取 使 QO1、Q;、Q1 十 0 和 势能 V 最 小 化 的 配置 。 
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12-34 3R 平面 机 械 手 静 力 学 。 
12-29 所 示 为 安装 到 墙 上 的 一 个 3R 平 面 机 械 手 。 确 定 每 个 关节 处 的 静 力 和 更 力 矩 以 
保持 机 械 手 的 状态 ， 如 果 g = 二 一 g "70。 


hy 





















































图 12-28 静止 条 件 下 安装 在 天 花 板 上 的 2R 平 机 械 手 图 12-29 ”安装 在 墙壁 上 的 3R 平 面 机 械 手 


友 12-35 ”关节 式 机 械 手 静 力 学 。 

一 个 R 上 RR 关节 式 机 械 手 如 图 5-22 所 示 。 对 于 g 二 一 g"ko。， 试 求 关节 处 的 静 力 和 静 
力矩 。 末 端 执行 器 承载 着 一 个 20kg 的 质量 块 ， 计 算 最 大 的 地 基 力 矩 。 

%& 12-36 SCARA 机 器 人 更 力学 。 

对 于 如 图 5-23 所 示 的 及 1RIRIP 关 节 SCARA 机 器 人 来 说 ， 如 果 g 王 一 go&u 和 末端 
执行 器 承载 着 一 个 10kg 的 质量 块 ， 试 计算 该 机 器 人 的 静态 关节 力学 系统 。 

412-37 球形 机 械 手 静 力学 。 

5-43 所 示 为 一 个 具有 球形 手腕 的 球形 机 械 手 。 如 果 末 端 执行 器 承载 着 一 个 12kg 的 质 
量 块 ， 分 析 该 机 械 手 ， 对 于 g 王 一 5&"&o 计算 关节 中 的 静 力 系统 。 

% 12-38 SRMS 机 械 手 静 力学 。 

航天 飞机 遥控 机 械 手 系统 (SRMS) 模型 如 图 5-24 所 示 。 分 析 SRMS 的 静态 配置 ， 对 
Fg=— gk) 计算 关节 力学 系统 。 

假设 连 杆 由 一 个 半径 + 二 0.25m MARRE m=12kg/m 的 均匀 圆柱 构成 。 假 设 SRMS 在 
无 重力 场 中 工作 ， 对 于 末端 执行 器 一 直 夹 持 着 一 个 24kg 的 质量 块 来 说 ， 利 用 表 5-11 中 所 示 
的 特征 求 得 地 基 力 学 系统 的 最 大 值 。 
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机 融 人 控制 是 跟踪 目标 运动 的 学 科 。 它 涉及 规定 机 器 人 运动 的 动力 学 和 和 运动 学 ， 还 包括 
确定 力 从 而 使 系统 具有 最 优 行为 的 优化 问题 。 一 个 典型 的 例子 就 是 ， 给 定 机 器 人 的 初始 配置 
和 最 终 配 置 ， 求 解 作用 于 机 器 人 上 的 力 ， 从 而 使 系统 在 最 短 时 间 内 完成 运动 。 

路 径 或 轨迹 规划 是 控制 技术 中 的 一 部 分 ， 在 这 种 技术 中 我 们 在 一 个 既定 的 时 间 表 内 规划 
机 械 手 的 运动 ， 在 关节 或 笛 卡 儿 空 间 中 规划 路 径 。 关 节 路 径 规划 直接 确定 关节 变量 的 时 间 变 
化 。 然 而 ， 稍 卡 儿 路 径 主 要 确定 终端 的 位 置 和 方向 。 因 此 ， 路 径 包 括 从 初始 配置 获得 一 个 期 
望 目标 ， 它 可 以 包括 避 障 。 关 节 路 径 规划 很 简单 ， 因 为 它 不 涉及 逆向 运动 学 ， 但 它 是 很 难 实 
现 机 械 手 在 笠 卡 儿 空 间 中 的 运动 。 然 而 ,第 卡 儿 坐 标 系 是 讲 得 通 的 ， 只 是 需要 道 向 运动 学 
计算 。 

在 这 部 分 中 ， 我 们 开发 相应 技术 以 获得 所 必需 的 指令 ， 从 而 控制 机 器 人 完成 作业 。 
























































第 13 章 





路 径 规 划 包 括 3 个 任务 : 中 定义 末端 执行 器 在 两 点 之 间 的 一 条 几何 曲线 ;外 定义 两 个 方 
向 之 间 的 旋转 运动 ; 定义 坐标 变化 在 两 个 给 定 值 之 间 的 一 个 时 间 函 数 。 所 有 这 3 个 定义 被 
称 为 路 径 规划 。 图 13-1 所 示 为 2R 机 械 手 末端 点 在 点 Po 和 Po 之 间 避 免 两 个 障碍 的 一 条 
路 径 。 











图 13-1 2R 机 械 手 末端 点 避免 两 个 障碍 的 一 条 路 径 


13.1 三 次 方 路 径 


三 次 方 函数 是 可 用 来 确定 位 于 两 个 给 定 值 之 间 变量 时 间 特 性 的 最 简单 多 项 式 。 
对 于 在 关节 空间 或 者 直角 坐标 空间 的 关节 变量 gt), 或 者 在 g(to) Maa) 两 点 之 间 
的 关节 变量 g(z)， 其 三 次 方 路 径 为 





























q (1)=ao | aıt? | aot? | ast? (13-1) 
这 里 
git? (to —3t1) Hqoti (3t0 —t1) qot +a ito 
ao 3 tots 2 (13-2) 
(t¢—to) (t¢—to) 
go—g1  . Vots(t7 +tots—2t2) +4 ot ti —ti— tott) 
ai =6tott E. 3 (13-3) 
(t¢—to) (t¢—to) 
do (3to +3t,)+91(—3to —3t1) q 1i FH tots — 2t) +40 (27 —t§ — tots) 
a2 a z (13-4) 
(Ci 一 上 0 ) (t¢—to) 


(CƏ) 
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_ 2qo—2G1 +40 Gito) +4 (tr—to) 
= Gito)’ 
gto) =o q (to =a 
q(t1)=gr q =qf 


iE AA : 


(13-5) 


(13-6) 





一 个 三 次 多 项 式 有 4 个 系数 。 因 此 ， 它 的 初始 点 和 终点 都 要 满足 位 置 和 


了 简单 起 见 ， 我 们 调用 变量 的 值 、 位 置 和 变量 的 速度 、 加 速度 。 假 设 一 个 





在 初始 时 刻 t。 和 末端 时 刻 tf 给 出 ， 例 如 式 〈13-6) 。 
以 边界 条 件 代替 关节 变量 的 位 置 和 速度 方程 ， 有 


7 





q(t)=ao Hait +a2t? +a3t? 





gq (1) =ai 十 2a2t 十 3a31? 











生成 4 个 路 径 系 数 方程 为 
l to to to \fao qo 
0 1 2to 3től|aı qo 
1 t @ tł Niles j qi 
O 1 2t, 322 J\43 qi 
其 解 在 式 (13-2) 一 式 (13-5) 中 已 给 出 。 
如 果 zo 二 0， 系 数 可 简化 为 
ag=ğy 
ai =qh 


O 3go) — (2a tq 





a2 














ʻi 
2(qi—qo) +go Hati 
a3 E 
也 有 可 能 采用 时 间 转 移 法 求 得 三 次 多 项 式 的 形式 
qd 人 Ci) 一 ao 十 al 一 10) 十 ao 一 10)2 十 as (t 
现在 ， 由 边界 条 件 式 〈13-6) 产生 一 方程 组 ， 有 
I 0 0 0 Q0 
0 0 0 ai 
1 (teto) (rato)? Gito)? | | as 
0 1 2(t1—to0) 3(t1—to0)?)\as 





速度 的 限制 。 为 





ZR 
之 


量 的 位 置 和 速度 





(13-7) 


(13-8) 


(13-9) 


(13-10) 


(13-11) 


(13-12) 


(13-13) 


(13-14) 


(13-15) 
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ao qo 
/ 
al qo 
= = 7 ， (13-16) 
az —(ti— to)? qo —3qi—2toqo —togit+2t1qo+trq 1) 
a3 (tf 一 上 0) 一 3(200 2qt toq tog’ | tiqo ttrq't) 





三 次 方 路 径 的 缺点 是 在 边界 处 会 有 一 个 导致 无 限 急 动 的 加 速 跳跃 。 
例 13-1 从 静止 到 静止 的 三 次 方 路 径 。 
假设 g(0)==10"、g (1)==45"、g (0) 二 9 (1) 二 0， 三 次 方 路 径 的 系数 为 : 
a0 王 10 ay=0 aa? 王 105 a3=—70 
(13-17) 
从 而 得 到 一 个 三 次 方 变量 路 径 为 
q(t) =10+105t? — 70t? 
(13-18) 








路 径 信 息 如 图 13-2 所 示 。 

例 13-2 ”终点 静止 的 三 次 方 路 径 。 

假设 关节 起 始 角 及 其 角速度 分 别 
为 gq(0) 二 10"、g (0)= 二 12*/s M q2) = 
45”"、g (2) 二 0， 该 运动 的 三 次 方 路 径 











q/() 4/1C)/s1, 4/1C)/s7] 




















系数 为 图 13-2 从 静止 到 静止 的 三 次 方 路 径 的 运动 学 曲线 
一 10 ai=12 af se 
nyum ai~ a2 2 a3 2 
(13-19) 
则 这 条 路 径 的 运动 学 方程 为 
q) =10+12t +40. 522-14. 523 (13-20) 
g(t) =81t—43. 542 +12 (13-21) 
q (t)=81—87t (13-22) 





其 相应 曲线 如 图 图 13-3 所 示 。 








4/C), 4/1C)/s1, AIC)Vs2] 














ern" pou Pera a es (er a ee er re ere ae pou pou basak pout 
0 02 04 06 08 1.0 12 14 16 18 2.0 
t/s 


图 13-3 关节 空间 中 到 静止 的 三 次 方 路 径 运 动 学 时 间 变 化 曲线 
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例 13-3 在 中 部 具有 定常 速度 的 从 静止 至 静止 路 径 。 


假设 我 们 需要 具有 给 定 的 定常 速度 9 二 9g. 的 一 个 从 静止 到 静止 的 路 径 ， 对 于 41 二 t 过 
tos H zo 过 i 过 1 过 tj 过 tt， 我 们 说 明 边 界 条 件 为 











gto) =o q (to) =q, 
g(t)=qg t1<t<te 
gt) qD =q (13-23) 


该 路 径 有 3 个 部 分 : 静止 到 定常 速度 再 到 静止 。 我 们 需要 一 个 运动 的 静止 至 部 分 方程 以 
实现 给 定 的 速度 。 














二 次 方 路 径 有 3 个 系数 ， 并 且 可 以 利用 其 满足 3 个 条 件 。 
q,(t)=aotaittazt? (13-24) 
g,(t)=ai+2a2t (13-25) 

这 些 条 件 是 初始 位 置 、 速 度 和 最 终 定 的 常 速 度 。 假 设 如 0， 静 止 到 路 径 的 条 件 为 
gi1(0)=g, gq1(0)=0 qi) =q (13-26) 
于 是 有 
do 二 Q0 (13-27) 
0=a] (13-28) 
qe =2aztı (13-29) 
因此 ， 从 静止 开始 的 路 径 为 
TO E (14, (13-30) 
1 





给 定 的 具体 定常 速度 qe 说明， 中 部 的 路 径 为 





g2(t)=q* (13-31) 
d.(t)=q'et HC ti<t<te (13-32) 
利用 + 二 ti 处 的 位 置 条 件 可 以 求 得 积分 常数 。 
otii =q'tı +C: (13-33) 
qh 
Cı =qo =t] (13-34) 


路 径 的 到 静止 部 分 有 4 个 条 件 。 因 此 ， 利 用 三 次 方程 和 边界 条 件 便 可 计算 。 三 次 方 
程 为 








q3 (t) =bo bit +b2t? +b3t? (13-35) 
q3 (t) =b +2b2t +3b3t? (13-36) 
边界 条 件 为 
qa (tp) =a (13-37) 
gs3(11) =0 (13-38) 
, 1, 
qs (2) =q2 (12) Tg 5q etz tqo L1G c (13-39) 
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g2(t2)=q2(t2)=q*e 
























































THA 
lote 好 tio qi 
O 1 2 3t?|| bi 
1 te £3 t} || be 7 qet2+qo Shia’ 
O 1 2t2 323) Les a 
其 解 为 
3. 2 
=a Te ee eae L | 
—2tzte tta tti to tt—3t2tf 3t2tt 
一 13 372tf 
加 t3 +t? — Stat Sts 
bi =qh Zisti tt 了 二 6q2t2 ti 
—2tzt Hti +t? C E — t Ht? — abate H Sen er 
tf 
ae t3 +t? —3toti a ei 
o —tet+2t; i —3t2—3tí l 
mmi thie te D Res ee 
3t2 十 3 
a t3 +t —3tot? +3t2ti 
; 
b3= a re |2 二 24 
2tetpts Hti torte — 3t2tf 3t2tt 
qt 
an t3 Ht? Sit ets 
对 于 





q/C) 


图 13-4 
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20} 


10 


中 部 具有 


t» <t<ti 





具有 下 列 值 的 路 径 曲 线 如 图 13-4 所 示 。 





50} 
40| 


30| 














02 l 0.4 06 | 08 T 10 
t/s 


常 速 度 的 部 分 从 静止 到 静止 路 径 时 间 关 系 曲线 


lay 


(13-40) 


(13-41) 


(13-42) 


(13-43) 


(13-44) 


(13-45) 


(13-46) 
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11 一 0. 4s t2=0.7s tr=Is 

ao 一 0 df 一 60” dv 一 50?/s (13-47) 
6113-4 通过 三 点 的 二 次 路 径 。 
通过 三 点 (qi. ti), (qz, t2) 和 (qs. t3) 的 二 次 路 径 为 


(t—t2)(t—t3) (t—t3)(t—-t) (ft1) Gt) 








Pom Gi Gan apa 
举 个 例子 ,通过 (10°, 0), (25°, 0.5) 和 (45°, D 的 二 次 路 径 为 
so Ot ET say 
= 一 (14. 072-19, 01—4.0) 
两 端 处 的 路 径 速 度 是 
q (0)=47. 5°/s (13-50) 
g (1)=—22. 5°/s (13-51) 


13.2 多 项 式 路 径 





对 于 g 二 gq (1) 来 说 ， 所 要 求 的 条 件数 确定 了 多 项 式 次 数 。 通 常情 况 下 ， 一 个 n 次 多 项 
式 路 径 为 














gq(1)=aodTaititast? ant” (13-52) 

解 该 方程 共 需 要 ?十 1 个 条 件 。 条 件 可 能 有 两 种 类 型 : 四 位 置 在 一 系列 点 处 ， 因 此 轨迹 将 通 
过 所 有 指定 点 ; 外 位 置 、 速 度 、 加 速度 和 两 点 之 间 的 加 加 速度 ， 因 此 可 以 控制 路 径 的 平滑 度 。 

寻求 多 项 式 系数 的 问题 ， 化 简 为 一 组 线性 代数 方程 组 ， 可 以 进行 数值 求解 。 然 而 ， 将 整 
个 路 径 分 割 成 一 系列 片段 ， 并 且 合 成 不 同 路 径 段 的 低 阶 多 项 式 ， 这 样 就 可 以 简化 路 径 规划 
了 。 这 时 必须 将 各 段 的 多 项 式 连接 起 来 ， 以 满足 所 有 要 求 的 边界 条 件 。 

例 13-5 5 次 多 项 式 路 径 。 

在 边界 处 ， 迫 使 一 变量 具有 指定 的 位 置 、 速 度 和 加 速度 ， 需 要 引入 6 个 条 件 : 
































q(t0)=go q (to) =qh q (to) =q 














gaN=qr QUD q Dq" (13-53) 
一 个 5 次 多 项 式 可 以 满足 这 些 条 件 。 
g(t)=aoatiast? t+a3t? tatt tast’ (13-54) 
并 且 产 生 一 个 由 6 个 方程 组 成 的 方程 组 。 
| 4 2. = t ” m 
1 2to 3 4t5 526 lla; qh 
O 2 6to 12t? 20t ||a, qh 
= (13-55) 
b- 2 no t? || as qi 
1 Bt; 3t? 4t? Bey ||a4 qi 























O 2 Bry 12%? Bory qt 


501 


和 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 








一 个 在 静止 位 置 处 没有 加 速度 并 且 具 有 下 列 条 件 的 从 停止 至 停止 的 路 径 ， 可 以 通过 一 组 
多 项 式 系数 方程 求解 。 








q(0)=10° Y(0)=0 q (0)=0 



































de g()=0 g (D)=0 (13-56) 
100 0 0 of A0 
0100 0 Offa 0 
002 0 0 olla? 0 
riad aí | las ee 
012 3 4 5 lla 0 
0 0 2 6 12 20)\a; 0 
求解 式 (13-57) 可 得 
Q0 10 
al 0 
a2 0 
z (13-58) 
a3 350 
aa) | 一 525 
ae 210 
这 时 ， 路 径 方程 为 
0(GD) 一 10 十 35013 一 52514 十 21015 (13-59) 


其 曲线 如 图 13-5 所 示 。 








4/°), W/1C)/s], 9/10 )/s21, Z/1C)/s3] 














图 13-5 从 停止 至 停止 的 5 次 多 项 式 曲线 





例 13-6 ”开始 -终端 路 径 处 的 零 加 加 速度 。 
为 了 形成 一 个 在 启动 和 终端 处 具有 零 加 加 速度 的 路 径 ， 一 个 7 次 多 项 式 


g(t) =ao raita azt? Fast? +a4tt +ast? Hast? Hart? (13-60) 


必须 使 用 8 个 边界 条 件 : 











q(O)=qo g(0)=0 gq (0)=0 q (0)=0 
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gq(l)=gr g(1)=0 q (1)=0 q (1)=0 








对 于 g(0) 二 10"、g (1) 二 45”"， 通过 求解 下 列 一 组 未 知 系数 a0o、a1l、: 














们 可 以 求 得 这 样 的 一 个 开始 -终端 零 加 加 速度 路 径 。 




















1000 0 0 0 oa A0 
0100 0 0 0 0lal lo 
0020 0 0 0 0dla| lo 
0006 0 0 0 oj lo 
1212122 1 1 1 |45 
0123 4 5 6 7l|as| lo 
002 6 12 20 30 42|la,| lo 
G 0 0 6 24 60 120 210jla;,]) \o 





由 式 (13-62) 可 求 得 多 项 式 为 


(13-61) 
~ a7 的 方程 ， 我 





(13-62) 





q(t) =10 +1225 —294025 + 
2450 — 700z" 
(13-63) 
该 多 项 式 的 路 径 曲线 如 图 13-6 所 示 。 
图 13-2 描述 了 有 关 加 速度 和 加 加 








速度 的 具有 无 条 件 的 从 停止 到 停止 运 
动 的 路 径 。 图 13-5 描述 了 迫使 运动 在 
开始 和 终端 处 有 零 加 速度 的 一 个 改善 。 





4/0), IC) F/1C)/821, 9/10/83] 











在 图 13-6 中 ， 迫 使 该 运动 在 开始 和 终 
端 处 具有 零 加 速度 和 零 加 加 速度 。 因 
此 ， 增 加 开始 和 终端 处 的 光滑 度 可 以 
增 大 加 速度 峰值 。 

例 13-7 Fe (ELT E E AE 








| q (to) | =ae 
前 半 段 的 运动 有 正 加 速度 ， 需 要 一 个 2 次 多 项 式 。 


gi (to) =act 
1 ; 
qı (10)= zaet’ tgo 
对 于 
1 
Ot Gtr 


基于 初始 条 件 ， 常 值 积分 为 





图 13-6 开始 -终端 处 零 加 加 速 


度 的 路 径 曲 线 


常 值 加 速度 路 径 有 具有 正人 负 加 速度 的 2 个 阶段 。 让 我 们 假设 正 负 加 速度 的 绝对 值 已 知 。 


(13-64) 


(13-65) 


(13-66) 


(13-67) 
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qi (0) =qos gq1(0)=0 (13-68) 
对 于 第 2 半 段 的 路 径 ， 我 们 可 以 2 次 多 项 式 开 始 
1 
q2 (t)=ao Hait +azt? zit (13-69) 
并 且 施 加 下 列 边界 条 件 : 
q2 (1)=gr 
g2(t)=0 
da a cee (13-70) 
gi 2 q2 2 gre f T qo 
这 些 条 件 产生 未 知 系数 的 3 个 方程 : 
1 tf t? ao at 
0 1 2t¢llar |= k (13-71) 
1 : 
0 1 zeja? gacti tao 
E » 
其 解 为 d 
1 之 f 
or 二 + a 
ao 5 40 
= l 2 > [ 
ay |= geti P20 之 [ 
a2 = [ 
ifi :0 
(二 + = t 
(13-72) = of 
常 值 加 速度 路 径 如 图 13-7 所 示 ， 之 
其 条 件 为 : qo=10°. g¢p=45°. ti 一 ]、 S% > r 
及 a.=200°/s?. 0.2 04 n 0.6 0.8 1.0 
例 13-8 点 序列 路 径 。 图 13-7 常 值 加 速度 路 径 曲线 
把 一 系列 点 编制 成 一 条 路 径 ， 变 量 必 须 在 指定 的 时 间 到 达 这 些 点 。 这 些 点 Wan 
成 近似 轨迹 。 考 虑 由 4 点 go、dl、d Maqa, 并 在 to、t1、t2 和 zs are 到 达 这 些 点 所 确定 
的 一 个 例子 。 除 了 位 置 之 处 ， 我 们 通常 对 初始 和 终端 速度 、 加 速度 施加 约束 。 对 于 这 样 的 一 
个 序列 点 ， 其 条 件 为 
q(t0)=go g (10)=0 家 (t0)=0 
gug (13-73) 
q (t2) =q2 
gq(13)=gs q(t3)=0 q (t3)=0 
利用 7 次 多 项 式 可 以 满足 这 些 8 个 条 件 。 
q(t)=ao aıt Fazzt? tast? Hatt Hast? Hast! ta7t! (13-74) 
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这 组 未 知 数 系数 的 方程 是 
z 

































































1 to t DO ty to ie ti 
2 3 4 5 6 on q0 
0 1 2to 3to 4t9 oto 619 Tto di 0 
O 0 2 6 120 200 30:6 42t5 ||a, 0 
ee ee A A A flal la en 
Li i B Oe 23 2 t3 llaa q2 
lt; #2 #2 «3 t3 z$ ts | {as q3 
O 1 2t3 3t? a. Be 6r? 7$ | 9 
2 3 4 5 | 27 0 
0 0 2 6t3 1213 2013 30t3 4213 
代入 数据 ， 将 其 简化 为 
1 0 0 0 0 0 0 0 ao 10 
0 iL 0 0 0 0 0 0 al 0 
0 0 2 0 0 0 0 a2 0 
1 0.4 0.42 0.4% 0.44 0.45 0.48 0.47 |] a3 加 20 (13985 
1 0.7 0.7% 0.7% 0.74 0.7% 0.75 0.77 |) aa 30 
1 1 1 1 1 1 1 as 45 
0 1 2 3 4 5 6 7 Ilas 0 
0 0 2 6 12 20 30 42 a7 0 
这 里 给 出 的 条 件数 据 为 
gq(0)=10” gqg(0)=0 q (0)=0 
q(0.4)=20 (13-77) 
q(0. 7) =30° 
q(1)=45° g(1)=0 q (1)=0 
其 系数 的 解 为 
ao 10 
aı 0 
a2 0 = 
as| | 1500.5 四 
= (13-78) >a 
a4 = (053 = 
:六 
Q5 12891 
as | | 一 10380 a 
ts 3076. 9 = 
这 些 系 数 的 多 项 式 产生 了 如 图 X 
13-8 所 示 的 多 项 式 曲 线路 径 。 





对 于 变量 g， 这 个 方法 提供 了 一 
个 连续 的 可 微分 函数 。g 二 g(t) 的 连 








图 13-8 


点 序列 路 径 曲 
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续 性 和 微分 性 是 一 个 优点 ， 它 提供 了 一 个 连续 的 速度 、 加 速度 和 加 加 速度 。 然 而 ， 方 程 数目 
会 随 着 序列 点 数 的 增加 而 增加 ， 它 需要 一 个 较 大 的 数据 存储 ， 增 加 了 计算 时 间 。 

例 13-9 ”将 一 路 径 分 割 成 一 系列 片段 。 

对 于 整个 轨迹 ， 我 们 可 以 将 路 径 分 割 成 一 些 片 段 ， 并 且 使 用 一 系列 低 次 的 多 项 式 ， 而 不 
是 使 用 一 个 单一 的 高 次 多 项 式 。 

考虑 下 列 边界 条 件 的 路 径 。 




















q(t0)=go q (to) =0 q (to) =0 (13-79) 


q(ta=qs Y=0 q (4) =0 
该 路 径 也 必须 通过 下 列 给 定 的 3 个 中 点 。 
q(t1)=g1, g(t2)=q2> g(t3)=g3 (13-80) 
让 我 们 将 整个 路 径 分 割 成 4 个 片段 : gq1 (1)、g2z (1)、g3(t) 和 ga(t)。 
qitt) 对 于 oo(o)<oliG)<oG) 和 1 二 :< 
gA) 对 于 gtid<qo@)<qt2) 和 titt 
qg) 对 于 gq@2.)<q3(t)<q(t3) 和 teo<t<tz 
qa) 对 于 qa) Kq Kq aa) 和 t3<t<ty 
第 1 段 的 边界 条 件 为 


























qi(to)=go gilio)=0 gi1(t0)=0 ETT 
qı (tı) =qı 
式 (13-81) 满足 一 个 3 次 方 函 数 。 
qi (t)=ao ta, (t—to) +a (t—to)? +a3 (一 io)3 (13-82) 
代入 边界 条 件 得 
1 0 0 0 ao qo 
0 1 0 0 al 0 
9 i m = À (13-83) 
1 (ti-to) (tı—to)? (ito)? la; qı 
由 式 (13-83) 可 得 
ao=qo 41=0 az:=0 a3 EA (13-84) 
第 2 个 段 的 边界 条 件 为 
g2@1)=q1 
g2(t1)=qilti) =a + 2a2(ti—to)?4 3aa(t1—to)?=go+3 7 
g2(t2) =q2 (13-85) 
一 个 2 次 多 项 式 将 满足 这 些 条 件 : 
d2() 一 00 十 01L 十 02t2 (13-86) 


代入 边界 条 件 得 
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1 z 好 Bo a 
0 1 wiet (13-87) 
1 zt t Jb? 二 
由 式 (13-87) 可 得 
t? —2tit: +t? te 一 qo 十 
ww = ee oer me e aa a en | | ee 
t t ti ttz 一 qo 十 
ONTA a, ee nae Fae EAC pon) eae 
2 1 —dot 
á a EH a fst le ce ee) Bes 
第 3 片段 的 边界 条 件 为 
qa (t2)=q2 qs(t2)=q2(t2)=bı 4 2bote (13-91) 
q3 (t3) =q3 
我 们 可 以 用 2 次 方程 满足 这 些 条 件 。 
q3(t)=co teit tct? (13-92) 
代入 边界 条 件 得 
1 zt 23 \fes q2 
0 1 2t||cı |=]|bı F2b:t2 (13-93) 
L- ee. T2 JANC q3 
ts t? iets +t? 
es 2 jee Cade as 一 2t2t3 十 t2 十 13 a —2tet3+e3+23 hee 
= rebates Wade on Pe Tae 2 
A ee agate) er ae | ae are. Gi 
第 4 片段 的 边界 条 件 为 
ga(t3)—gs  qaults)=q3(t3)=c1 +2cats po 
qa lta)=qa qata) =0 gq alts) =0 
4 次 多 项 式 可 以 满足 式 (13-97), 
qa (@)=dotdittdo2t? +ds3t?+dat' (13-98) 
代入 边界 条 件 得 
Lt ee at 尖 on a 
O 1 2 35 43 [ldi ae re 
1 to 12 B a lS qa (13-99) 
0 1 2 3t? 4tł [|ds 0 
0 0 2 6t 1242 Ja 0 
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例如 ， 给 定 的 一 组 条 件 
to=0 ż1=0.4 t2=0.7 t3=0.9 #4=1 (13-100) 
q(0)=10° 4(0)=0 q (0)=0 
q (0. 4)=20° 
q (0.7)=30° (13-101) 
q(0. 9)=35° 


q(D=45° 9:(D=0 9 ()=0 


则 有 
qi(t)=10+156. 2523 (13-102) 
q2(4)=—41. 564222. 78t—172. 2t? (13-103) 
q3(t) = 148. 99—321. 67t +216. 672? (13-104) 


q(t) =198545—827166. 6672t + 129050022 —893500t3 +231666. 6724 (13-105) 

其 曲线 图 如 图 13-9 所 示 。 

片段 法 的 缺点 就 是 缺乏 一 个 光滑 
的 整体 路 径 ， 并 且 有 一 个 不 连续 的 加 
速度 。 为 了 增加 路 径 的 光滑 性 ， 我 们 
需要 使 用 高 次 多 项 式 ， 并 把 约束 施加 
到 加 速度 和 可 能 的 加 加 速度 突变 上 。 

式 (13-102) ~ Ųų (13-105 ) 
说 明 : 


qı (41) =875 q (t1) =—344. 4 





45F 





























0 0.2 0.4 0.6 0.8 1.0 


(13-106) f l t/s 
q(t2)=—344.4 qatr) =433. 34 图 13-9 将 一 个 路 径 分 割 成 一 系列 片段 
(13-107) 
qa (t3)=433.34 gy (43) =7900 (13-108) 
G14 dott) utvr 94(ts)F gy Cts) (13-109) 


因此 ， 路 径 的 加 速度 在 连接 点 处 不 是 连续 的 ， 并 且 显 示 一 个 有 限 的 跳跃 。 加 速度 中 的 跳 
跃 引 入 了 一 个 无 限 的 加 加 速度 。 对 于 路 径 光 滑 度 来 说 ， 具 有 连续 的 加 速度 是 一 个 最 低 要 求 。 
具有 连续 加 速度 的 分 段 路 径 被 称 为 样 条 线 。 

KH 13-10 最 小 二 乘法 多 项 式 。 

当 近 似 化 轨迹 的 点 数目 太 大 时 ,我们 可 以 使 用 低 次 多 项 式 交 近 于 这 些 点 。 最 小 二 乘法 是 
一 个 可 被 应 用 的 方法 ， 该 方法 可 以 确定 表述 近似 化 路 径 的 所 选择 的 多 项 式 系数 。 

考虑 一 个 给 定 N 个 点 的 路 径 。 


pi=p(ti) i=l, 2, 3, =, N (13-110) 


Bits n 次 多 项 式 可 近似 化 给 定 的 路 径 。 如 果 NN 三 n 十 1， 这 时 多 项 式 确切 通过 所 有 
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soe we Gi 
给 定 的 点 。 为 了 应 用 低 次 多 项 式 ， 我 们 选择 n<N+1 
g=aytaittazt? + +a pt” (13-111) 
“AT N 个 点 的 式 (13-110) 和 式 (13-111) 时 ,我 们 在 ti 时 刻 定义 一 个 误差 ei, 
=pi-—qi=pi-ao—ait; azt? ant? (13-112) 
所 有 点 pi 汪 之 和 是 总 误差 ce， 即 
N N 
= p9 a= 5 (piao Taiti azt? aat (13-113) 
i=] i=] 
最 小 误差 。 提供 了 最 好 的 近似 化 式 13-11) 的 途径 。 在 最 小 值 处 ， 所 有 的 偏 导数 ae/ 
Pao, Pe/Aai, *, APe/Aan 消失 。 这 些 条 件 会 产生 2 十 1 个 方程 。 
ae a ‘ 
一 一 2 》) (piao Taiti azti ant) )=0 
Jao i=l 
a =) se (pi-ao ait; azt? ant”) =0 (13-114) 
ga i=1 
=p (pi-—ao—aiti azt? Gat, )=0 
he i 
每 个 方程 除 以 一 2， 重 新 调整 2 十 1 个 方程 并 同时 求解 系数 ai G=, 2, s n), 
N N N 
aoN+a, X titta, >= >») pi 
i=l i=l i=l 
N N N N 
ao 5 itai 5 tite tan 5 pti D tipi 
i=l i=l i=l i=1 
N N N N 
ao ay D i ey, > = >) tp (13-115) 
i=l i=l i=1 i=l 
重新 调整 可 得 一 组 用 于 求解 ci i=l, 2, +, n) 的 线性 方程 。 
Aa =b (13-116) 
这 里 
N N N 
N Siti Dr Dz 
i=l i=1 i=1 
N N N N 
2 > 
i=l i=l i=l i=1 
A= (13-117) 


N 
Dat 


i=l 
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Db 
i=l 

ao ee 

ai p=] 

a 三 | 放学 b= F 

i=l 

an 
N 
Dt: 


13.3 非 多 项 式 路 径 规 划 


(13-118) 


TERY PBA TE TRB LS H, SEAN E CS PR CE SCG AE. HEIR PR AE ZR PR 





数 是 最 常用 的 路 径 曲 线 。 


g(t) =ao +aicosazt +a3sinast 





q(t) =ao +a\t—az2sina3t 


然而 ， 我 们 也 可 以 使 用 其 他 的 函数 近似 化 方法 ， 例 如 傅 里 叶 级 数 : 


Ao 


qt 一 十 5 [A,cos(nx) +B, sin(nz) ] 


n=l 


1 fx 
Ao=— |" q(t) dt 
1 fx 
A,=—|" q(t )cos(nx ) dt 
1 fx i 
B,=— |" q(t) sin(nx ) dt 
WETE (Lengdre) 函数 : 


dn (t) = 5 L;i(@t)q@i) 
i=0 


n 





Ei; 
L,Q= II ， i=0. 1, Zacks 


a pei Y 
WHER (Chebyshev) 函数 ; 
gn+1(t)=2tqn (1t)—gqgn—1(t) 
qo(t)=1 qi(t)=t 
例 13-11 谐 波 路 径 。 
考虑 两 点 g(to) Maa 之 间 的 谐 波 路 径 


q(t)=aotaicosa2st Hassina2t 


式 (13-129) 具有 从 静止 到 静止 的 边界 条 件 。 


g(to)=qo gq (to)=0 
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(13-119) 
(13-120) 


(13-121) 


(13-122) 


(13-123) 


(13-124) 


(13-125) 


(13-126) 


(13-127) 
(13-128) 


(13-129) 
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将 这 些 条 件 应 用 于 式 





q@n=qr q(tr)=0 
(13-129), ， 可 得 解 为 





1 
q(t)= 2 G go~ (gi— qa) cós 


4/ C), 4/1C)/s], 4/1/82 

















a) 
tf to 


(13-130) 


(13-131) 




















0 0.2 





























04 06, “08 1.0 
t/s 
图 13-10” 谐 波 路 径 曲 线 
对 于 下 列 数 值 ， 其 解 的 曲线 图 如 
图 13-10 所 示 。 
to =0 tr=1 7 
o 1 pS 
qo=10 qo=0 L 
™~ 
df 一 45” df 一 0 (13-132) 5, 
例 13-12 ERI. 
具有 下 列 从 静止 到 静止 边界 条 件 悦 
的 在 两 点 g(to) 和 g(t1) 之 间 的 摆 线 = 
路 径 为 
gto) =qo g (to) =0 
qq q(t) =0 (13-133) 图 13-11” 摆 线 路 径 曲 线 
z — 2 
os ee ofz to) LG. x(t £2) EELT 
T t{— to 2 ZLf to 
对 于 下 列 数 值 ， 其 摆 线 路 径 曲 线 如 图 13-11 所 示 。 
to =0 tr=1 
eee (13-135) 
qi=45° dg4 一 0 


比较 图 
径 曲 线 。 








13-11 和 图 13-5 可 知 ， 摆 线路 径 的 主要 运动 特性 类 似 于 5 次 的 从 静止 到 静止 的 路 
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13.4 基于 关节 路 径 规 划 的 机 械 手 运动 





关节 变量 作为 时 间 函 数 ， 可 使 用 机 械 手 的 正 疝 运动 学 求解 ， 这 允许 我 们 确定 末端 执行 器 
的 运动 路 径 。 
例 13-13 基于 关节 路 径 的 2R 机 械 手 运动 。 
假设 依据 三 次 方 函 数 ， 我 们 已 经 计算 了 2R 机 械 手 两 关节 的 路 径 ， 它 们 是 
01(t) 二 10 十 1057? 一 7073 (13-136) 
62 (t) =10+350¢° —525¢4 +2108? (13-137) 











关节 路 径 满足 下 列 条 件 : 
6:(0)=10° 01(0)=0 
01(1) 一 45" 01(1)=0 (13-138) 
8.(0)=10° 62(0)=0 62(0)=0 
62 (1) =45° co 0 2(1)=0 (13-139) 
2R 机 械 手 正 向 运动 学 在 例 5-8 FERI, R 


cCO1 +82) F02) 0 lichi +lzc(01 +62) 
SCO1 +02) c(O, +02) 0 lishi +l2s(01 +42) 
1 























T 一 0T117T， = (13-140) 
0 0 0 
0 0 0 1 
OT: 的 第 4 列 说 明基 体 坐 标 系 中 机 械 手 末端 点 的 笛 卡 儿 位 置 ， 因 此 末端 点 的 X、Y 分 量 为 
一 /1cos0O1 十 /2cos(CO1 +62) (13-141) 
Y=1,sinO; +lesin(O; +02) (13-142) 


取代 式 (13-136) 和 式 (13-137) 中 的 91 和 0 ， 可 得 末端 点 位 置 的 时 间 变 化 曲线 。 对 
于 1 一 42 一 lm， 这 些 变化 曲线 如 图 13-12 所 示 ， 与 此 同时 机 械 手 在 初始 位 置 和 终端 位 置 的 
组 态 如 图 13-13 所 示 。 





























图 13-12 2R 平 面 机 械 手 末端 点 位 置 的 X、 立 分 量 曲线 图 13-13 2R 机 械 手 在 起 始 位 置 和 最 终 位 置 处 的 配置 
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BRA. KE 


关节 的 首次 转向 并 不 是 非常 





sE 
变量 ， 





变量 总 是 及 时 地 





只 要 定义 并 给 定 作为 时 间 函 数 的 关节 
量 坐标 是 相对 坐标 ， 机 器 人 的 最 终 组 态 是 相同 的 ， 它 们 甚至 可 以 一 起 转向 。 
通过 应 用 一 组 关节 路 径 移 动机 器 人 也 不 总 是 一 个 恰当 的 方法 。 如 果 关 节 变 
变化 和 浮动 ， 定 义 关节 路 径 将 是 比较 复杂 的 。 而 且 ， 通 过 定义 关节 路 径 按 照 所 期 望 的 几何 路 








径 移动 机 器 人 末端 执行 器 ， 这 并 不 是 容易 的 。 
沿 着 一 条 线 移动 的 2R 机 械 手 。 
(13-143) 














例 13-14 
让 我 们 考虑 具有 下 列 数值 的 2R 机 械 手 
11=/12=0. 25 
假设 其 末端 点 按照 一 条 给 定 的 如 图 13-14 所 示 的 直线 Y= f(X) Boh. WA 
Y=—0. 25998X +0. 3705 (13-144) 
假设 在 10s 内 ， 第 1 个 角 基 于 3 次 方 路 径 曲 线 在 45°23 135" 之 间 移 动 ， 即 
45°<01 <135° (13-145) 
T 3n 元 
= | 2 3 一 
0 1 300° 1000° 0<t<10s (13-146) 
肘 关 节 R 沿 着 圆 移动 ， 并 且 其 初始 位 置 坐标 为 
XR 一 0. 25cos =. 17678 (13-147) 
(13-148) 





Yri=0. 25sin T=. 17678 

















E Ri 的 距离 d=0.25 处 ， 点 Pi 必须 位 于 式 (13-144) 所 描述 的 直线 上 
- (Y—0. 17678)? (13-149) 























TEH 
d =/(X — 0. 17678)? 4 
= /(X —0. 17678)? +(—0. 25998X +0. 3705 一 0. 17678)? =0. 25 
因此 ， 点 Pi 的 位 置 坐标 为 
X pp =0.41122 Ypl 王 0. 26359 (13-150) 
其 起 始 角 wp Al 02 为 
= Yrpi—Xpi _ 0. 26359—0. 17678 _ E i 
ọ=arctan ka Ka =arctan 0.41122 —0. 17678 0. 35463rad=20. 319° (13-151) 
02 =01—9= (=—o. 35463 ]rad=0. 43077rad=24. 681° (13-152) 
肘 关 节 R 的 终点 位 置 位 于 
3 
X r =0. 25cos = —0. 17678 (13-153) 
(13-154) 


3 

Yr =0. 25sin 7 =0. 17678 
E R: 的 距离 d=0.25 处 ， 点 P， 必 须 位 于 式 (13-144) 所 描述 的 直线 上 

+ (Y—0. 17678)? (13-155) 






































TEH 
d =/(X +0. 17678)? 4 
= /(X +0. 17678)? + (—0. 25998X +0. 3705—0. 17678)? 一 0. 25 
因此 ， 点 Po 的 位 置 坐标 
X p? =2. 8188X107?  Yp2=0. 37783 (13-156) 
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终点 处 的 角 p 和 0 为 





aii T m 0. 37783—0. 17678 
Pp arctan yoy ome 8188X1020. 17678 (13-157) 
一 0.93432radsxz53. 533° 
3 
0 一 9 一 p 一 二 一 0.93432 一 1. 4219rad~81. 469° (13-158) 





为 了 确定 运动 期 间 的 角度 0 ， 我 们 应 该 遵从 相同 的 过 程 。 我 们 求 得 作为 0; 函数 的 肘 关 
WR 的 位 置 。 





XR=0.25cos01 Yr=0. 25sin01 (13-159) 
在 距 肘 关节 R 的 距离 d=0. 25 处 ， 末 端点 Pi 必须 位 于 式 (13-144) 所 描述 的 直线 上 。 
d = J/(X p —0. 25cos01)? + CY —0. 25sind, )? 



































= /(X p —0. 25c0s6, )? + (— 0. 25998X p +0. 3705—0. 25sind; )? =0. 25 
(13-160) 
方程 解 出 Xp FRAR (13-144) 中 ， 则 可 得 末端 点 P 在 运动 期 间 的 坐标 (Xp, Yr). 
这 时 ， 角 度 yp 和 0, 为 





Yp 一 入 Yp 一 0. 25sin01 





=. R Ls 
g=arctan Xp Xk arctan Xp 0.2503, (13-161) 
加 7 Yp —0. 25sin@1 
0 一 0 一 0 二 0 OD a e o (13-162) 


KHH, ATES P 沿 着 式 (13-144) 所 描述 的 直线 移动 ， 此 时 91 随 着 式 (13-146) 的 
变化 而 变化 ， 和 角度 9。 必须 根据 式 (13-162) 变化 。 






































=X 





图 13-14 沿 着 给 定 直 线 移动 的 2R 机 械 手 





13.5 fi KIL be we 


FAR LA ERKI BS FE BA EE PG eS KRR. BER te A AT at AY AA hs 
和 终止 点 坐标 分 别 为 
Po=Po(Xo, Yo, Zo) Pi=Pi(Xi1, Yı, 21) (13-163) 
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通过 几何 空间 曲线 ， 我 们 可 以 连接 这 些 点 。 
Z=Z(X) Y=Y(X) (13-164) 








ix 
X(t10)=Xo XGN=X; (13-165) 

这 时 ， 对 于 每 个 坐标 ， 我 们 可 以 在 Po 和 Pi 之 间 定 义 一 个 时 间 路 径 X， 以 便 确 定 几何 
路 径 式 (13-164) 上 其 他 坐标 的 运动 行为 。 

将 点 连接 起 来 ， 或 者 设计 一 条 路 径 通过 接近 于 但 不 必 通 过 这 些 点 ， 也 可 以 规划 一 条 点 到 
点 的 路 径 。 一 个 实际 的 方法 就 是 利用 具有 常 值 速度 的 一 条 直线 设计 一 条 路 径 ， 并 且 变 形 形 成 
一 个 平稳 过 渡 的 拐角 。 

分 段 运 动 设计 连接 点 ro 和 点 r; 并 且 接 近 于 变换 曲线 上 拐角 ri 的 路 径 。 
































fiS i f 
r(t)=r, (ri—ro) toSt Stı —t 
ti —to 
(¢—t1—t')? Sieur. 
Ne 人 < / 
r(t) rı Mt’ (1 h) rı roa 4t Ca t1) r2 rı) ti t StS, +t 
ti —t / 
r(t)=ri 了 (r2—r1) ti tt tts 
2 1 
(13-166) 
路 径 在 to 时 刻 从 ro 开始 ， 沿 着 一 条 f 


线 以 常 值 速度 mw = 站 一 一 运动 ， 终 止 于 转 
) 

换 时 刻 1 一 1 的 点 。 这 时 路 径 切 换 到 常 值 
加 速度 抛物 线 。 在 1 十 1 时 刻 的 另 一 转换 
点 处 ， 路 径 切换 到 第 2 条 线 ， 并 且 以 常 值 


速度 wm 一 站 一 一 朝 着 目标 点 ro 运动。 时 间 


1 一 to 是 从 ro 运动 到 ri 的 要 求 时 间 ， 时 间 
tz 一 t1 是 从 ri 运动 到 rs 的 要 求 时 间 ， 如果 
没有 转换 路 径 的 话 。 该 路 径 在 原理 上 如 
A 过 渡 抛物 线 作为 一 条 路 径 

证 明 : 

第 1 条 线段 在 to 时刻 从 点 ro 开始 ， 
没有 任何 变形 ,在 时 刻 以 常 值 速度 到 达 点 ri1 。 第 2 条 线段 在 1s 时 刻 以 常 值 速度 结束 在 点 
rih, KWAZ, Æ ti 时 刻 从 点 ri 开始。 























图 13-15 第 卡 儿 空间 中 现 两 条 线段 之 间 的 

















r= (13-167) 


arr (rm 一 rr) att: 
i—i 


到 达 nn 之前， 我 们 引入 一 个 间隔 时 间 上 ， 以 便 从 直线 转换 到 一 条 变换 曲线 。 这 时 变换 曲 
线 位 于 时 间 二 一 #' 和 时 间 嫩 十 t' 之 间 。 最 简单 的 变换 曲线 是 一 个 抛物 线 ， 在 末端 点 处 与 直线 
具有 相同 的 速度 。 














515 


D 应 用 机 强人 学 : 运动 学 、 动 力学 与 控制 技术 





在 第 1 条 曲线 和 第 2 条 曲线 上 ， 变 换 曲 线 的 边界 位 置 分 别 位 于 


/ 



























































rt =n : 1 (13-168) 
tı = to 
rintin o (13-169) 
£914 
这 里 
6, 一 r1 一 ro (13-170) 
6. 一 mr 一 站 (13-171) 
变换 曲线 起 始点 和 终止 点 处 的 速度 分 别 为 
r(t1—t’)= ô: (13-172) 
t1— to 
rite’) = ô: (13-173) 
t2—t] 
假设 沿 着 过 渡 曲 线 的 运动 加 速度 是 常 值 ， 即 
(1) 二 .二 常数 (13-174) 
因此 ， 积 分 之 后 的 变换 曲线 为 
fixe y 1 / nie 
Fi) 一 PC 一 上 ) 十 (一 三 十 ra )+ 2 tt red f (13-175) 
代入 式 (13-168) MIÈ (13-172)， 可 得 
t=ti 1 ; 
r(t)=ri+ ôt (@¢—t, tt’)? r (13-176) 
ti —to 2 
4 t= tem, BMhAra) 必须 位 于 其 终点 。 
reit =r, t+—— 8) =n. +6, 4:2 7,2? (13-177) 
t2—t] tı — to 
因此 ， 曲 线 上 的 加 速度 为 
| a 四 } (13-178) 
© 2t \te—-t, tito 
因此 ， 曲 线 方程 变 成 了 
CE ta” «ba ae)? 
rt)=rı— ô: war i) t 4 i) (13-179) 
式 (13-179) 表明 ，ro 和 产 之 间 的 路 径 具 有 式 (13-166) 中 所 给 定 的 分 段 特 征 。 
机 械 手 遵从 第 卡 儿 路 径 ， 再 加 上 沿 着 路 径 的 时 间 表 便 可 确定 终点 坐标 系 的 位 置 和 方向 。 





笛 卡 儿 路 径 规划 中 的 观点 包括 : 从 初始 点 到 达 一 个 指定 的 目标 、 避 障 及 机 械 手 能 力 范 围 之 内 

















的 停留 。 利 用 被 称 为 控制 点 的 2 个 点 建 模 一 条 路 径 。 控制 点 通过 直线 连接 起 来 ， 














外 将 执行 瞬时 抛物 线路 径 。 





RTRA 


ASAT EAC ES Ta A E H AA NS GERA BCP a E a A 


近 控 制 点 的 连续 路 径 。 
$i 13-15 2D 笛 卡 儿 空 间 中 的 路 径 。 





考虑 XY 平面 中 连接 点 (1，0) 和 点 (1，1) 的 一 条 直线 和 连接 点 (1，1) 和 点 O, 
1) 的 男 一 条 直线 。 假 设 在 点 d. O 处 的 时 间 为 零 ， 点 (1，1) 处 的 时 间 为 1s， 点 CO, 
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1) 处 的 时 间 为 2s。 对 于 时 间 间 隔 二 0. 1s， 控 制 点 处 的 位 置 矢量 为 
ro=i (13-180) 
n=it; (13-181) 
r: =j (13-182) 
rti >t) =r “6 i+ h “\; (13-183) 
t] t] 
r+ =n “6:-(1 “i h (13-184) 
t2 ty 
这 里 
Ôi 一 r 一 ro 一) (13-185) 
=r: -r =i (13-186) 
这 时 ， 运 动 路 径 可 用 如 图 13-16 所 示 的 分 段 函 数 进行 表述 ， 即 
i 十 地 0<t<0. 9 
ra)= h i+ a "i 0.9<¢<1.1 (13-187) 
(2 一 六 ;十 7 1. 1<t<2 
沿 着 路 径 的 运动 速度 也 是 一 个 分 段 函 。 
数 ， 即 
j 0<t<0. 9 
ra)= ee a le 0 gp, 1 
一 : 1. 122 
(13-188) 
例 13-16 ” 沿 着 一 条 直线 路 径 的 2R 机 
械 手 。 
假设 图 13-14 所 示 的 2R 机 械 手 ， 有 
11=1s=0. 25 (13-189) x 


H BREH RA mA E E R Y= f CX) 
和 运动。 直线 方程 为 








Y= f(X)=—0. 25998X +0. 3705 
在 10s 内 ， 机 械 手 从 Pi 移动 到 Po. 

Xp, 一 0.41122 Yp, 一 0.26359 

Xp, 一 一 2. 8188 Yp,=0.37783 
对 于 X， 让 我 们 定义 一 个 从 静止 到 静止 的 三 次 方 路 径 曲 线 。 


X 一 0.41122 一 0.01149096z2 +0. 00076606423 








图 13-16 连接 两 条 直线 的 变换 抛物 线 


(13-190) 


(13-191) 
(13-192) 


(13-193) 


通过 替换 式 (13-190) 中 X= 二 X(t)， 我 们 可 以 确定 作为 时 间 z 的 函数 的 Y 路 径 曲 线 方 


程 为 


Y=0. 26359 十 2. 9874X10-3t2 一 1. 9916X10 一 4z3 


(13-194) 
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友 13.6 旋转 坐标 路 径 规 划 


假设 在 zo 时刻， 末端 执 行 器 在 其 初始 方向 上 有 一 个 旋转 变换 矩阵 SRo。。 在 tf PA, R 
端 执行 器 必须 位 于 其 最 终 的 方位 SRr:。 由 角 轴 旋转 矩阵 R a.g 定义 旋转 路 径 。 
Ru， 一 "RiI 一 CRUGR: (13-195) 
式 (13-195) 将 在 最 终 方 位 SRr 上 的 末端 执行 器 坐标 系 变换 到 初始 方位 SRu， 表 示 在 初 
始 坐 标 系 中 的 单位 矢量 定义 旋转 轴 "w， 因 此 从 初始 方位 到 最 终 方位 所 要 求 的 旋转 矩阵 将 为 
Ro,, =°RiCRo (13-196) 


保持 "x 为 常量 ,我 们 可 以 定义 在 zf 时 刻 从 SR。 BCR, 的 变换 和 矩阵 RO, Ke 的 角 




















路 径 








为 了 控制 旋转 运动 ,我们 可 以 定义 在 初始 定位 与 最 终 定位 之 间 的 一 系列 控制 定位 矩 
阵 SR1、SR2、…、SR,， 通 过 控制 定位 矩阵 来 旋转 末端 执行 器 坐标 系 。 当 在 初始 定位 和 终端 
定位 之 间 有 一 个 控制 定位 矩阵 SR 时 ， 这 时 利用 角 轴 旋转 矩阵 RO,, 可 将 初始 的 定位 矩 
MES Ry 变换 到 控制 定位 矩阵 SR1， 接 着 再 利用 第 2 角 轴 旋转 矩阵 Rh ,将 控制 定位 矩阵 SR: 



































变换 到 最 终 定位 。 
Rosgo ="R SR (13-197) 
Rigg SCRISORI (13-198) 
证 明 : 
根据 罗 德 里 格 斯 旋转 公式 [ 式 (3-4) ]， 有 
OR =Ro, , =Icos$ tu uTversg using (13-199) 
将 坐标 系 Bi 转换 到 另 一 坐标 系 Bo 的 角 及 轴 可 从 式 (13-200) M (13-201) 中 求 得 。 
1 
cosg =F LtrC Ri) —1] (13-200) 
~ 1 
05, = 0 0pT = 
u aa R; ) (13-201) 


WFR FE RES Ro 是 将 坐标 系 Bo 变换 到 全 局 坐标 系 G WIERE, JHOR 是 将 坐标 系 Bi 
变换 到 全 局 坐标 系 G 的 旋转 矩阵 ， 则 有 
GR ;一 GRJU0R， (13-202) 








因此 ， 有 
ORI=Ro,, = ROR (13-203) 
通过 改变 绕 着 旋转 轴 的 旋转 角 ， 我 们 定义 一 个 线性 时 间 独 立 变量 的 旋转 矩阵: 
rut) rie) ris(t) 
PRO) =R (0) 5 = rat) r2) ra @)| totes (13-204) 
r3) r3 @) r3 (t) 
这 里 ，zo 是 当 末端 执行 器 坐标 系 位 于 定向 SRo 时 的 时 间 ， t1 是 当 末端 执行 器 坐标 系 位 于 定 
HSR: 时 的 时 间 ， 并 且 有 
t—to 
Jp teos{ =") 





t—t 
tp—t 








9 0 
rıl (1) =ujvers{ 
0 
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t—to t—to 













































































21(1) 二 s | i 
r21 uj u? Vers =. $ Tussin a $ 
t—t t—t 
r31 (4) =u, u3 vers " ġ— uzsin $ (13-205) 
tf{—to t{—to 
t—t t—t 
ri2(t) =u ue2vers 2 $ġ— uzsin ° $ 
t{— to tt to 
F t—t t—t 
pa 0) =ubvers{ : Jp reos{ : Jy 
tt to tt Lo 
t—t t—t 
r32(t) =u2u3vers " $ tusin ° $ (13-206) 
tt to tt to 
t—t t—t 
rı3 (t) =u, u3 vers ° |g +uzsin ° $ 
tr to tr to 
t—t t—t 
r23 (t) =u2u3 vers ° ġ— usin ° $ 
tf—to t{—to 
t—t t—t 
ra (1) =u vers{ ° Jy | cos[ : Jy (13-207) 
tt to tt — to 
HEBER Ct) 可 以 将 最 终 的 坐标 系 绕 着 旋转 轴 "v 变换 到 最 初 的 坐标 系 之 中 ， 因 此 有 
SR 一 cRooRICL) (13-208) 
如 果 在 最 初 定向 和 最 终 定向 之 间 有 一 个 控制 定向 坐标 系 >SR1， 那 么 有 
SR1=°RoR (13-209) 
SR;=°R!R (13-210) 
因此 ， 有 
Ror, =9R1 =R] ER, (13-211) 
Riss, ='Rr=oR{ SR, (13-212) 





利用 式 (13-213), WeFe Fa keEOR, FR, 可 以 作为 线性 时 变 旋转 和 矩阵 进行 定义 。 
"Ri 一 及 oo (=) 4 to tA 
REE) =R e, (2) tı tSt: (13-213) 
Ail FA LE SE RAT AKR i ITAR RA RIE SOR 绕 着 "x 轴 旋 转 以 获得 控 
制定 向 矩阵 SR; ， 这 时 将 末端 执行 器 坐标 系 绕 着 1x 轴 旋 转 则 可 获得 最 终 的 定向 矩阵 Ri。 
按照 13.5 节 中 的 抛物 线 变 换 技 术 ， 我 们 可 以 定义 一 个 连接 5R。 和 SRr 并 且 通 过 或 接近 
于 角 方向 SR1 的 方向 路 径 ， 该 方向 路 径 位 于 一 瞬时 旋转 路 径 上 。 方 向 路 径 在 to 时刻 从 SR 
开始 ， 绕 着 一 轴 以 恒定 角速度 旋转 直到 :一 :上 一刀 时 刻 为 止 。 在 这 个 时 刻 ， 路 径 切换 到 一 个 具 
有 恒定 角 加 速度 的 旋转 抛物 线路 径 中 。 在 1 二 ti 十 t 时刻， 在 另 一 变换 方向 上 ， 路 径 切 换 到 
了 第 2 条 中 路 径 并 且 以 恒定 速度 朝 着 目标 方向 “Rir 旋转 。 如 果 没 有 转换 路 径 ， 则 时 间 i 一 zo 
是 从 SRo 移动 到 “Ri 的 要 求 时 间 ， 时 间 如 一 与 是 从 “Ri 移动 到 5 Ri 的 要 求 时 间 。 
在 到 达 定 向 “Ri 之 前 ， 我 们 引入 一 个 时 间 间 隔 刀 以 便 从 第 1 条 路 径 片 段 切 换 到 过 渡 路 径 
中 。 这 时 ， 过 渡 路 径 位 于 时 间 2) 一 汪 和 时 间 姜 十 志 之 间 。 在 第 2 切换 方向 时 刻 ， 过 渡 路 径 以 
与 第 3 条 路 径 片 段 相同 的 角速度 结 
位 于 第 1 条 路 径 片 段 和 第 3 条 路 径 之 间 的 过 渡 路 径 的 边界 位 置 分 别 为 
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ER Gi =t) = R R; Gi ) =F RoRo. q- t Js t=t t (13-214) 
ER Gi HO =F Ri R Gi HO SSR Ri) g, t=ti +2’ (13-215) 
trey 
这 时 ， 过 渡 路 径 为 


Ry (2) =R R: ( 








tiot t Gott), (Ort)? ， ， 
) | ! t1—t Kt tt 


2t” 4t (tito) f 4t et) 
=G i (+t 142 
Ro Roe (17 4t Ct] e)no R la, (H +) A (13-216) 
则 整个 路 径 为 
R(t) =°R1 (4) SRo, (= to -J 40 totti 一 上 
， (13-217) 
R(t) =R, (t) ft Siti tt 


RÆ SRG) =Ri,. (=) ti Ht <t<to 
例 13-17 绕 着 Z 轴 的 旋转 。 
考虑 一 刚体 B， 在 1 二 0 时 刚体 坐标 系 最 初 与 全 局 坐标 系 重 合 。 因 此 ， 它 的 最 初 的 变换 
和 矩阵 是 一 个 单位 矩阵 。 


GR, =I (13-218) 
在 10s 之 后 ， 假设 刚 体 B 位 于 SR;。 
一 1 0 
GRs=| 0 一 1 0 (13-219) 
0 0 1 





旋转 矩阵 ?Ri 的 旋转 轴 是 Z 轴 ， 旋 转角 是 x*。 刚 体 By 和 Bo 在 最 初 方向 和 最 终 方向 之 间 
的 变换 矩阵 为 

















=] 0 0 
2R1=SRISR»=| 0 一 1 0 (13-220) 
0 0 1 
对 于 旋转 角 a， 让 我 们 定义 一 个 三 次 方 从 静止 到 静止 路 径 。 
_3t , T} - 
a = Toot 500! (13-221) 


刚体 By 和 Bo 之 间 的 刚体 B 角度 路 径 为 


3m, T 3 3 T 3 
2 ES 43) 0 
cos t 500° ) ~ sin ~ 500° 








cosa —sina 0 
? R1 =| sina cosa 0|=| . (3x T 3x T 
5 i i sin( ert — 8 cos ee) 0 
0 0 1 
(13-222) 
例 13-18 Sea X 轴 的 旋转 。 
假设 刚体 B 最 初 位 于 “Ri 。 
1 0 0 
T .Xx 
op OT ST (13-223) 
l T 
0 sin To cos 10 
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在 10s 内 ， 假 设 刚 体位 于 SR，。 


il 0 0 
T .xX 
Bp 8S SND (13-224) 
0 sin cos = 
2 2 





旋转 矩阵 *R1 的 旋转 轴 是 X 轴 ， 旋转 有 角 是 2 < 一 至 一 至。 对 旋转 角 yY， 我 们 定义 一 个 三 

















5 2 10 
次 方 从 静止 到 静止 路 径 ， 即 
y= po T N09) 
以 确定 全 局 坐标 系 G 和 Bo 坐标 系 之 间 的 刚体 BARRE. BNA 
1 0 0 
ER: = f cosy ar (13-226) 
0 siny cosy 


在 任意 时 刻 上， 相对 于 Bi 的 刚体 B 位 于 'R， 
1 0 0 
nn 0. 95lcosy —0. 309siny 一 0. erin (13-227) 
0 0.309cosy+0. 95lsiny 0. 95lcosy—0. 309siny 


13.7 末端 执行 器 路 径 的 机 械 手 运动 


笛 卡 儿 路 径 规划 是 路 径 规划 最 适合 的 应 用 。 如 果 工 业 机 器 人 的 主要 工作 是 选择 和 放置 运 
动 ， 那 么 对 于 在 基体 坐标 系 的 3 维 空间 中 的 末端 执行 器 来 说 我 们 必须 决定 一 个 期 望 的 几何 路 
径 。 这 时 ， 我 们 可 以 定义 一 个 单个 坐标 的 时 间 路 径 X， 通 过 使 用 几何 路 径 确 定 其 他 坐标 的 
时 间 变 化 曲线 。 当 已 知 末 端 执 行 器 坐标 的 时 间 函 数 时 ， 我们 便 可 以 决定 末端 执行 器 的 速度 、 
加 速度 和 加 加 速度 行为 。 

逆向 和 运动 学 是 确定 关节 变量 的 运动 学 。 取 代 动 态 和 运动 方程 中 关节 变量 的 位 置 、 速 度 和 加 速度 ， 
则 可 求 得 执行 器 所 要 求 的 转 矩 或 力 ， 该 转 矩 或 力 使 未 端 执 行 器 沿 着 具有 规划 运动 的 期 望 路 径 移 动 。 

几何 笛 卡 儿 路 径 是 路 径 规划 在 机 器 人 学 中 的 一 个 应 用 方法 ， 因 为 它 能 控制 机 械 手 抓 取 物 
体 时 所 用 力 和 波动 的 大 小 。 第 卡 儿 空间 中 的 路 径 规划 也 可 决定 外 部 几何 约束 。 然 而 ,一 条 笛 
卡 儿 路 径 需 要 逆向 运动 学 以 决定 关节 变量 的 时 间 变 化 曲线 。 

例 13-19 用 于 所 设计 的 笛 卡 儿 路 径 的 关节 路 径 。 

考虑 在 一 条 直线 了 二 1.5 上 从 点 (1，1.5) 到 (一 1，1.5) 的 从 静止 到 静止 的 笛 卡 儿 路 
径 。3 次 多 项 式 可 以 满足 初始 点 和 最 终点 处 的 位 置 约束 和 速度 约束 。 

X(0)=Xo=1  X(0)=Xo,=0 






























































(13-228) 
X()=X;=-1 X(1)=X:=0 
多 项 式 的 系数 为 








ao=l ai=0O az 6 a3z=4 (13-229) 
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笛 卡 儿 路 径 为 
X =1—6t? +4¢° 
Y=1.5 
2R 平面 机 械 手 的 逆向 运动 学 在 例 6-3 中 已 被 计算 了 ， 有 具体 为 


(litla) —(X?+Y") 
0 一 二 2atan2 | : : 











(X?+Y?2)—(1, 12)? 
X (lı +12cos02) +Yl2sind2 
Y (11 +12 cos) — X12 sind» 





01 =atan2 








这 里 ,符号 土 说 明 机 械 手 的 提 肝 和 垂 肝 情况。 依靠 在 点 d, 1.5) 处 的 起 始 位 置 ， 假 设 机 械 




















(13-230) 
(13-231) 


(13-232) 


(13-233) 





手 位 于 那个 位 置 上 。 让 我 们 考虑 提 肘 的 情况 ， 此 时 我 们 仅仅 接受 关节 值 属于 提 肘 配置 的 那些 
值 。 将 式 (13-230) MIÈ (13-231) 代入 式 (13-232) 和 式 〈13-233) ， 可 得 关节 空间 中 的 路 





径 为 











{2 td. )?—(4t3 — 6t? +2. 5)? 

(403 — 622 +1)? —C11 —l2)? 

(1— 62? +4t°) Ci +l2cos02) +1. 522 sind2 
1. 5¢€2; +l2cos02) —(1— 6t? +42? le sind, 
在 总 时 间 的 每 隔 1/30 时 刻 处 ， 机 械 手 的 曲线 如 图 13-17 所 示 。 








01 =atan2 





























图 13-17 当 末 端点 沿 着 直线 y=1. 5 移动 时 2R 机 械 手 的 曲线 














例 13-20 沿 着 一 条 直线 的 2R 机 械 手 。 
考虑 具有 下 列 值 的 如 图 13-14 所 示 的 2R 机 械 手 。 
11=12=0.25m 
在 10s 内 ， 假 设 其 末梢 点 在 给 定 直线 上 位 于 点 Pi MP: 之 间 移 动 。 
Y 一 一 0. 25998X 十 0. 3705 
Xp, =0.41122 Yp, 一 0.26359 
Xp, =—0. 0282 Yp,=0. 37783 





























(13-234) 


(13-235) 


(13-236) 


(13-237) 
(13-238) 
(13-239) 


对 于 坐标 XX， 定义 一 个 3 次 方 的 从 静止 到 静止 路 径 ， 我们 决定 末梢 点 的 笛 卡 儿 路 径 。 





X =0. 41122—0. 01318262? +0. 00087884z3 


522 





(13-240) 


第 13 章 new Gi 








端点 的 运动 曲线 如 


Y=0.26359 十 0. 0034272? 一 0. 000228482° (13-241) 


图 13-18 一 图 13-20 所 示 。 





利用 式 (6-39) 和 式 (6-42) 的 逆向 运动 学 ,我们 可 以 求 得 关节 角度 的 变化 ， 如 图 13-21 


所 示 。 
让 我 们 将 运动 的 整个 时 间 分 成 n= 二 40 等 份 。 机 械 手 在 每 个 时 间 步 的 配置 如 图 13-22 
所 示 。 






































图 13-18 末梢 点 的 笛 卡 儿 坐 标 时 间 变 化 曲 





—0.02 


速度 /(m/s) 


一 0.04 


—0.06 

















图 13-19 末梢 点 的 速度 分 量 时 间 变 化 曲线 








加 速度 /m/s?) 




















图 13-20 末梢 点 的 加 速度 分 量 时 间 变 化 出 








Éa 
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图 13-21 


例 13-21 沿 着 一 条 直线 的 没有 末端 点 
加 速度 的 2R 机 械 手 。 

考虑 具有 下 列 值 的 如 图 13-14 所 示 的 
2R 机 械 手 。 








lı =l,=0. 25m (13-242) 
在 10s 内 ， 假 设 其 末梢 点 在 给 定 直 线 上 
位 于 点 Pi 和 P， 之 间 移 动 。 

Y=—0. 25998X +0. 3705 (13-243) 
Xp, =0.41122 Yp, =0. 26359 (13-244) 
X p,=—0.0282 Yp, =0. 37783 (13-245) 
对 于 坐标 xX, ELA 5 次 方 路 径 ， 以 

便 在 两 个 端点 处 应 用 零 加 速度 。 








X =0. 41122—0. 0043942t +0. 0006591324 —0. 00002636522" 





2R 机 械 手 关节 旋转 角 




















SS 




















将 X RAR (13-243) 中 ， 我 们 也 能 确定 坐标 Y 的 表达 式 为 


Y=0. 26359 十 0. 001142423 —0. 0001713624 +0. 0000068544¢° 


1 0 01 02 03 04 * 

图 13-22 在 42 个 等 时 间 间 隔 处 2R 机 械 手 的 配置 
(13-246) 
(13-247) 





FFA A JL) tt (13-246) 和 式 〈13-247) ， 我 们 确定 末梢 点 的 运动 曲线 如 图 13-23 ~ 





图 12-25 所 示 。 





利用 式 (6-39) 和 式 (6-42)， 我 们 可 以 求 得 关节 旋转 角 的 变化 曲线 如 图 13-26 所 示 。 




















0 2 4 





图 13-23 
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0.02 





速度 /(m/s) 


—0.04 


-0.06 | 








一 0.08 




















图 13-24 末梢 点 速度 分 量 在 零 终点 加 速度 路 径 上 的 时 间 变 化 曲线 








加 速度 /(m/s”) 


























图 13-25 末梢 点 加 速度 分 量 在 零 终点 加 速度 路 径 上 的 时 间 变 化 曲线 





角度 /(*) 














0 2 4 6 8 10 
t/s 


图 13-26 2R 机 械 手 关节 旋转 角 在 零 终 点 加 速度 路 径 上 的 时 间 变 化 曲线 
































例 13-22 沿 着 一 条 直线 运动 晶 没 有 末端 点 加 速度 的 2R 机 械 手 。 
考虑 具有 下 列 参 数值 的 如 图 13-27 所 示 的 等 臂 长 2R 机 械 手 。 














lı =l: =0. 25m (13-248) 
在 10s 内 ， 假 设 其 末梢 点 在 给 定 直 线 上 于 点 Pi MP: 之 间 移 动 。 
Xp,=0.41122 Yp, 一 0.26359 (13-249) 
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Xp, =—0. 0282 Yp,=0.37783 (13-250) 
然而 ， 在 点 Ps 处 有 一 个 圆 形 禁止 区 域 ， 末梢 点 不 能 通过 该 区 域 。 

Xp,=0.19151 Yp,=0.32071 (13-251) 

(X—Xp,)? +(Y—Yp, )? =0. 025? (13-252) 


为 了 找到 点 Pi 和 点 Ps 之 间 绕 开 点 P 的 路 径 ， 让 我 们 在 Pi1P， 的 平分 线 上 选择 一 段 
HA BLOEI. K 13-27 给 出 了 一 些 可 选择 的 路 径 。 圆 弧 必 须 位 于 机 械 手 的 工作 空间 之 
内 ， 它 是 绕 着 基体 点 的 一 个 圆 环 。 








(11 一 07)2<X2 十 Y2< (11 十 102)? (13-253) 
O<X?+Y? <0. 5? (13-254) 
圆 形 路 径 的 圆心 应 该 处 在 下 面 的 直线 上 。 
Y—Yp, 一 3.8464(X 一 XP ) (13-255) 
让 我 们 选择 一 点 Pc ， 作 为 圆 形 路 径 的 圆心 。 
Xc=0.1 Yc 王 一 0.06 (13-256) 
因此 ， 路 径 方 程 为 
(X—-Xc)?+(V¥—-Yc)? =0. 45? (13-257) 


该 路 径 如 图 13-27 所 示 的 虚线 。 
对 于 X， 我 们 使 用 一 个 5 次 方 路 径 以 便 在 两 个 端点 处 应 用 零 加 速度 。 
X 一 0.41122 一 0.004394213 +0.0006591324 —0.000026365225 (13-258) 




















图 13-27 在 Ps 点 处 , 点 Pi 和 P， 之 间 且 绕 过 禁止 区 域 的 圆 形 路 径 














EX 代入 式 (13-257) 中 ， 可 得 Y 的 时 间 路 径 方 程 为 
Y=Yct+/0. 45?-—(X—Xc)? (13-259) 
末梢 点 的 运动 曲线 如 图 13-28 一 图 13-30 Pras. sk (6-39) 和 式 (6-42) 给 出 了 如 
图 13-31 所 示 的 关节 转 旋 角 。 在 42 个 等 时 间 步 处 ， 机 械 手 配置 如 图 13-32 所 示 。 
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图 13-28 在 圆 形 路 径 上 末梢 点 的 第 卡 儿 坐标 时 间 曲 线 























速度 /(m/s) 














图 13-29 在 圆 形 路 径 上 末梢 点 速度 分 量 时 间 变 化 曲线 











加 速度 /(m/s”) 














图 13-30 ”在 圆 形 路 径 上 末梢 点 加 速度 分 量 时 间 变 化 曲线 
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图 13-31 关节 旋转 角 的 时 间 变 化 曲线 








Pn Pn PT PE a a E 
一 0.1 0.1 0.2 0.3 04 X 





图 13-32 在 42 个 等 时 间 间 隔 步 上 2R 机 械 手 在 圆 形 路 径 上 的 配置 











例 13-23 在 一 条 直线 上 的 关节 式 机 械 手 。 
13-33 所 示 为 了 一 个 关节 式 机 械 手 。 假 设 ， 








图 13-33 关节 式 机 械 手 
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1,=0.5m 12=1.0m ls=1.0m (13-260) 
假设 机 械 手 的 末梢 点 在 10s 内 从 位 于 点 Pi 移动 到 点 Po 
1.5 一 1.0 
rp =|0.0| rp =| 1.0 (13-261) 
1.0 1.5 
对 坐标 XX， 利用 5 次 方 多 项 式 路 径 ， 我 们 求 得 下 列 X 有 关 时 间 变 化 的 函数 表达 式 。 
X =1.5—0.02523 +0.00375t4 —0.0001525 (13-262) 
让 我 们 用 直线 将 点 Pi 和 点 Po 连接 起 来 ， 确 定 坐 标 Y、Z 有 关 时 间 自 变量 的 表达 式 为 
Yp,—Y 
Y=Yp, +" (X— Xp, )=0. 01023 —0,001524 +0.0000625 (13-263) 
Xp, Xp, 
Zp,—ZpP, 二 
Z=Zp, (X—X p, )=1+0. 005¢3—0.00075t! +0.0000325 (13-264) 
Xp, Xp, 


利用 逆向 运动 学 方程 ， 我 们 可 以 求 得 机 械 手 关节 变量 的 时 间 变 化 曲线 如 图 13-34 所 示 。 











lı —Z +l2sinð: 
Oy arccos (1E snt) — 9, (13-265) 
一 C? 十 VC2 一 Cl1Cs 
02 = 2arctan C (13-266) 
1 





H/C) 

















图 13-34 关节 式 机 械 手 关节 变量 的 时 间 变 化 曲线 


Y 
arctan x X £0 











01 二 y (13-267) 
arctan x" X=0 
| | 2l2X | X? | 
Cr=4 22 oa 73 + oa z? (13-268) 
Co =2lil2 — 2l Z (13-269) 
2l X X? 
—72 [72 24 | 2 = 
C3 Li 21,;Z | L3 cosO1 13 I cos201 l Z (13 270) 
13.8 ”本章 小 结 
串 行 机 器 人 可 以 被 认为 是 连 杆 的 可 变 几 何 链 ， 该 几何 链 将 未 端 执行 器 的 组 态 与 附 有 基体 
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坐标 系 的 笛 卡 儿 坐 标 系 联系 起 来 ; 正 向 和 运动 是 通过 已 知 关节 坐标 来 求 得 末端 执行 器 组 态 的 数 
学 几何 关系 ; 逆向 运动 学 则 是 对 于 给 定 的 末端 执行 需 组 态 来 求 得 关节 坐标 的 数学 几何 关系 。 

末端 执行 器 的 笛 卡 儿 运 动 路 径 必 须 作 为 时 间 函 数 而 被 表达 ， 以 求 得 连 杆 的 速度 和 加 速 
度 。 能 够 求 得 从 项 止 到 静止 运动 的 第 1 个 应 用 路 径 函 数 是 一 个 3 次 方 路 径 函 数 ， 对 于 在 两 个 
给 定点 q (to) Al; (tr) 之 间 的 变量 gq; (1) 来 说 ， 有 

di (iD) 一 ao 十 ait 十 aot2 dast’ (13-271) 

通过 在 路 径 上 的 某 些 点 上 增加 要 求 ， 例 如 零 加 速度 或 者 加 加 速度 等 ， 我 们 需要 使 用 高 次 
多 项 式 以 满足 条 件 。 一 个 n 次 多 项 式 可 以 满足 nn 十 1 个 条 件 。 也 可 能 将 一 个 多 条 件 路 径 分 割 
成 一 些 具 有 和 较 少 条 件 的 间隔 (片段 )。 区 间 路 径 必须 被 连接 起 来 以 满足 它们 的 界 边 条 件 。 

一 条 运动 路 径 也 可 以 基于 不 同 的 数学 隐 数 而 被 定义 。 谐 波 函 数 和 摆 线 函数 是 最 常用 的 路 
径 函 数 。 非 多 项 式 方程 的 主要 优点 是 有 比较 简单 的 表达 式 ， 其 缺点 是 非 线 性 。 

当 在 关节 坐标 空间 或 者 省 卡 儿 坐标 空间 中 的 一 条 运动 路 径 需 要 被 定义 时 ， 必 须 利 用 正 向 
运动 学 和 逆向 运动 学 以 求 得 其 他 空间 中 的 运动 路 径 表 达 式 。 

末端 执行 器 绕 着 脐 点 的 旋转 动作 需要 一 个 旋转 路 径 。 利 用 罗 德 里 格 斯 公式 和 旋转 矩阵 可 
以 用 类 似 于 笛 卡 儿 路 径 的 方法 在 数学 上 定义 旋转 路 径 。 












































习题 


13-1 标记 与 符号 。 
描述 下 列 这 些 符 号 的 含义 。 
1) to Dt Dga) 4) +z" 

13-2 ”从 静止 到 静止 三 次 方 路 径 。 

试 求 满足 下 列 条 件 的 关节 坐标 系 的 三 次 方 路 径 : 

1) g(0)=—10°, g(1)=45°, g(0)=g(1)=0, 

2) q(0)=0°, g(1)=50°, g(0)=g(1)=0。 

3) g(0)=10°, g(1)=60°, g(0)=q(10) =0, 

13-3 到 静止 路 径 。 

试 求 一 个 满足 下 列 条 件 的 二 次 方 路 径 : 

gq (0)=—10", g(1)=45” 4(1)=0 

利用 二 次 方 路 径 计 算 该 路 径 的 初始 速度 。 然 后 求 一 个 满足 与 二 次 方 路 径 相 同 的 边界 条 件 
的 三 次 方 路 径 。 比 较 这 两 种 路 径 的 最 大 加 速度 。 

13-4 定常 速度 路 径 。 

计算 一 条 满足 下 列 条 件 的 路 径 : 

q(0)=—10°, gq) =45°, d 0O)=ġ4 10) =0, FF AWE RUE dg =25°/s FE 12° A 35° ZS lal 
移动 。 

13-5 定常 加 速度 路 径 。 

计算 以 定常 速度 4 一 25"/s 在 12 和 35" 之 间 移 动 的 一 条 路 径 ， 该 路 径 应 同时 满足 下 列 
条 件 : 














q(0)=—10°, g(1)=45°, ¢(0)=6(10)=0, 
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13-6 ALTMAN DE BEER 


试 求 一 条 满足 下 列 边 界 条 件 的 路 径 : 
gq (0) 二 0"、g(1) 二 66"、g9(0) 二 9(1) 二 0 并且 在 开始 点 、 中 点 和 终点 处 有 零 加 加 速度 。 





13-7 控制 点 。 
试 求 一 条 满足 下 列 条 件 的 路 径 : 
q(0)=10°, g(1)=95°, g(O)=g(1)=0, JF] 





日 通过 下 列 控制 点 : 











q(0. 25) =30°, g(0.5)=65° 


13-8 在 起 始 - 中 间 - 停 止 路 径 处 的 零 加 加 速度 。 
为 了 使 下 列 一 条 路 径 的 加 加 速度 尽 可 能 地 接近 零 ， 可 以 使 用 8 次 方 多 项 式 和 9 个 边界 





条 件 。 
os art 1 agt? 





g(t) =ao Hait Fat? 


9 个 边界 条 件 为 
g(0)=0 4gO)=0 0)=0 70)=0 
g (0.5) =0 


g(1)=120° Gg(1)=0  ğa)=0 gM)=0 


13-9 点 序列 路 径 。 
这 里 给 定 一 个 序列 点 的 条 件 ， 试 求 满足 下 列 给 定 条 件 的 一 条 路 径 。 








1) 
q(0)=5° g(O)=0  ä(0)=0 
q (0. 4) =35° 
q (0.75) =65° 
g(1)=100° Gg(1)=0 g(1)=0 
2) 
g(0)=5° g(O)=0 g(0)=0 
q(2)=15° 
q(4) =35° 
q (7.5) =65° 
qg(10)=100° g(10)=0 + g(10)=0 
13-10 将 一 条 路 径 分 制 成 若干 段 。 
利用 分 割 法 ， 对 于 下 列 条 件 ， 试 求 一 条 路 径 ， 
q(0)=5° q(0)=0 g (0) =0 
q(2)=15° 
q(4)=35° 
(7.5) =65° 
q(10)=100° ¢(10) =0 g (10) =0 
通过 将 整个 路 径 分 割 成 4 段 。 
对 于 第 1 段 g1(1): q(O<qi @)<q(2) HO<t<2, 
H2<:<4., 





对 于 第 2 段 g2 (1): q(2)<q2)<q(4)] 
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对 于 第 3 段 g;3(1): g(4)<gs(t) 过 gq(7.5) 且 4 过 Zt 过 7.5。 

WF 4 Be qi): q(7.5)<qu(t)<q(10) H7. 5 过 :过 10。 

Ww13-11 附加 条 件 。 

在 分 割 法 中 为 了 形成 一 条 光滑 的 整体 路 径 ， 我 们 增加 附加 条 件 以 匹配 分 段 。 求 解 各 有 段 之 
间 光 滑 过 度 ( 零 冲击 过 度 ) 的 路 径 。 

W13-12 ”最 小 二 乘法 路 径 。 

利用 最 小 ens 试 求 最 好 的 次 多 项 式 路 径 ， 该 路 径 近 似 于 由 下 列 点 所 构成 的 路 径 。 

gq(0)=5° gq(1)=7°” q(2)=15° g(3)=21” q(4)=35° 9 (7.5)=65° g(9)=85° 
gq (10)=100° 

1) n=2, 2) n=3. 3) n=4, 4) n=5。 

W13-13 最 小 二 乘法 路 径 和 边界 条 件 。 

利用 最 小 二 乘法 试 求 最 好 的 n 次 多 项 式 路 径 ， 该 路 径 近 似 于 由 下 列 点 和 条 件 所 确定 的 
HE 














q(0)=5° q(0)=0 g (0) =0 
q(2)=15° 
q(4) =35° 
q(7.5) =65° 
q(10)=100° g(10)=0 9g (10)=0 
1) n=2, 2) n=38, 3) n=4, 4) n=5, 
13-14 ARPI 2R 机 械 手 运动 。 
如 有 果 关 节 变 量 遵 从 给 定 的 路 径 , 试 求 具有 1 二 /2 一 1m 的 2R 机 械 手 端点 路 径 。 
01(t)=10 十 156. 2523 
O24) =—41. 564222. 782 —172.2t? 
13-15 遵从 关节 路 径 的 3R 平面 机 械 手 运动 。 
试 求 具有 下 列 参 数 的 3R 平面 机 械 手 端点 路 径 。 
li 二 lm /一 0.65Dm /3=0.35m 
如 果 关 节 变 量 遵从 下 列 路 径 : 
6, (£1) = —41.56+222.78t —172.2t? 
02 4) =148.99— 321.672 + 216.6727 
63 (t) = 198545 —827166.6672¢ + 1290500r? —893500r? + 231666.672' 
计算 端点 的 最 大 加 速度 和 端点 加 加 速度 。 
13-16 R FR || R 关节 式 机 械 臂 运动 。 
试 求 如 图 5-22 所 示 的 关节 式 机 械 手 端点 的 笛 卡 儿 轨 迹 ， 其 几何 参数 为 
di 二 lm d2=0 /一 lm /3 一 1m 























关节 路 径 为 
01 (@)=—41.56+222,78¢ 一 172.27? 
02 (1)=148.99—321.67t 二 216.67z7? 
03 (t) =198545 —827166.6672¢ + 129050042 一 893500z3 十 231666.67z4 
13-17 第 卡 儿 路 径 。 
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用 直线 将 下 列 点 连接 起 来 。 在 上 一 1s 内 对 于 从 静止 到 静止 路 径 ， 确 定 作为 时 间 函 数 的 笛 
卡 儿 坐标 。 

1) Pi 一 (1.5，1.5)，P: 一 (一 0.5，1.5)。 

2) Pı=(0, 0), Ps=(1, 1-5). 

3) Pi=(—1.5; 1); Ps=(0.5, 1.5) 

4) Pi=(—1.5, 1, 0), Ps=(0.5, 1.5, 1). 

5) Pı=(—1, By —1), P=(—0.5, 1.5, 1). 

13-18 2R 机 械 手 笛 卡 儿 路 径 。 

考虑 2R 平面 机 械 手 。 

1) 计算 笛 卡 儿 空 间 中 的 一 个 3 次 方 从 静止 到 静止 路 径 ， 以 用 直线 将 下 列 点 连接 起 来 。 

Pi=(1.5, 1) P=(—0.5, 1.5) 

2) 计算 并 绘制 具有 ji 二 1 二 lm， 且 遵从 第 卡 儿 路 径 的 机 械 手 关节 坐标 。 

3) 计算 关节 变量 的 最 大 角 加 速度 。 

13-19 3R 机 械 手 笛 卡 儿 路 径 。 

考虑 具有 =l: =l; =1m 的 3R 关节 式 机 械 手 。 

D 计算 笛 卡 儿 空 间 中 的 一 个 3 次 方 从 静止 到 静止 路 径 ， 以 用 直线 将 下 列 点 连接 起 来 。 

Pi=(—1.5, 1, 0) P=(0.5, 1.5, 1) 

2) 计算 和 绘制 遵从 笛 卡 儿 路 径 的 机 械 手 关节 坐标 。 

D 计算 关节 变量 的 最 大 角速度 和 最 大 角 加 速度 。 

友 13-20 ”对 于 给 定 笛 卡 儿 路 径 的 关节 路 径 。 

假设 2R 机 械 手 的 端点 以 定常 速度 v= 二 1m/s 在 由 两 个 半圆 构成 的 路 径 上 从 点 Pi 移动 到 
点 P，。， 如 图 13-35 所 示 。 两 个 半圆 的 圆心 分 别 位 于 (0.75m，0.5m) 和 “(一 0.75m， 
0.5m) 处 。 

1) WR 有 1 二 1 二 1m， 计 算 并 绘制 关节 路 径 。 

2) 计算 关节 变量 中 最 大 角速度 的 值 及 其 位 置 。 

3) 计算 关节 变量 中 最 大 角 加 速度 的 值 及 其 位 置 。 

4) 计算 关节 变量 中 最 大 角 加 加 速度 的 值 及 其 位 置 。 












































~ 


一 一 一 


图 13-35 在 由 两 个 半圆 所 构 的 路 径 上 移动 的 2R BLA 


H 

















k 13-21 避 障 和 路 径 规划 。 
让 我 们 确定 点 Pi;=(1.5，1) 和 Pi 二 (一 1]，1) 之 间 的 路 径 以 便 避 障 ， 如 图 13-36 所 
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示 。 这 条 路 径 由 在 中 间 处 具有 过 渡 圆 形 路 径 的 两 条 直线 所 构成 。 圆 半径 7 二 0.5m， 圆心 位 
于 障碍 物 的 低 点 人 处。 直线 光滑 地 连接 到 圆 上 。 





(-0.25,1) (0.25,1) 


(0.75, 0) 





OP, 











图 13-36 2R 机 械 手 在 笛 卡 儿 运 动 空间 中 的 一 个 障碍 物 























RA 1) =l2=1m 的 2R 机 械 手 端点 在 Pi 点 处 从 静止 开始 ， 以 定常 加 速度 沿 着 第 1 条 线 
移动 。 端 点 在 圆 形 路 径 上 移动 时 保持 定常 速度 v 王 lm/s， 然 后 以 定常 加 速度 移 到 最 终 直线 
片段 停止 于 P， 点 处 。 

1) 计算 和 绘制 关节 路 径 。 

2) 试 求 两 个 关节 变量 最 大 角速度 的 值 和 位 置 。 

3) 试 求 两 个 关节 变量 最 大 角 加 速度 的 值 和 位 置 。 

4) 试 求 两 个 关节 变量 最 大 角 加 加 速度 的 值 和 位 置 。 

妈 13-22 ”给 定 笛 卡 儿 路 径 的 关节 路 径 。 

1) 用 一 条 直线 连接 点 Pi 二 (1.1,0.8,0.5) 和 了 Ps 二 (一 1,1,0.35)。 

2) 试 求 从 静止 到 静止 的 3 RTT BR. Zell VE H BS Ta] es CY TP RL AB hs X.Y AZ. 

3) 计算 如 图 5-22 所 示 的 关节 式 机 械 手 的 关节 路 径 ， 如 果 几 何 参 数 为 

dij=lm d2=0 lo=lm 13=l1m 

4) 试 求 关 节 变 量 的 最 大 角速度 、 最 大 角 加 速度 、 最 大 角 加 加 速度 的 值 及 其 位 置 。 

W13-23 ”抛物线 过 渡 。 

在 习题 13-21 中 ,将 Po 二 (0,0.6) 作 为 一 个 拐角 ， 用 两 条 直线 连接 点 Pi 二 (1.5,1) 和 
Ps 二 (一 1,1)。 如 果 运 动 的 总 时 间 是 12s， 并 且 有 下 列 间 隔 时 间 ， 请 设计 一 条 抛物 线 过 渡 路 
径 以 便 避 开 拐 角 。 

1) 间隔 时 间 2’=1s, 

2) 间隔 时 间 忆 一 2s。 

3) 间隔 时 间 t =5s. 

4) 间隔 时 间 t 二 8s。 

5) 间隔 时 间 1 =10s, 

W13-24 ”旋转 路 径 。 

将 一 刚体 坐标 系 B 绕 着 Z 轴 旋 转 90"。 根 据 以 下 要 求 ， 确 定 旋转 变换 矩阵 Rap(z) 。 

1) 在 :一 1s 内 旋转 ， 角 速度 是 定 值 。 

2) 在 :一 1s 内 旋转 ， 旋 转 从 静止 到 静止 。 

W13-25 组 合 的 旋转 路 径 。 

将 一 刚体 坐标 系 B 绕 着 Z 轴 旋 转 90"， 绕 着 X 轴 旋 转 60°. 
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1) 确定 旋转 变换 矩阵 SRs(t1)， 以 使 刚体 在 1 二 1s 内 绕 着 Z 轴 旋 转 从 静止 到 静止 ， 然 
后 在 ts 二 1s 内 绕 着 X 轴 旋 转 从 静止 到 静止 。 

2) 两 个 旋转 矩阵 Rx (1) 和 Rz(t) WR. MECRO) 有 唯一 的 一 个 时 间 变 量 ， 那 么 
在 :一 1s 之 后 坐标 系 B 将 在 哪儿 呢 ? 

3) 两 个 旋转 和 矩阵 Rx (4) MR) MR, WHER, ,并 确定 SRs 的 旋转 角 和 旋转 轴 。 针 
IKRA BE tSls 内 从 最 初 方 向 移动 到 最 终 方向 的 旋转 角 ， 定义 从 静止 到 静止 的 路 径 。 

13-26 ERA FA HERE PEE 

假设 6 自由 度 机 器 人 的 球形 手腕 从 静止 位 置 开 始 ， 在 最 终 坐 标 系 Be 中 依次 围绕 着 z 轴 、 
x 轴 、z 轴 旋 转 g= 二 15"、9 二 38" 和 y= 二 77*"， 坐 标 系 Be 固定 在 手腕 点 处 。 

1) 如 果 每 个 旋转 占用 1s 时 间 ， 请 设计 pgp、9、y 的 一 个 从 静止 到 静止 的 3 次 方 旋转 
































路 径 。 
2) 试 求 将 手腕 从 最 初 方向 移动 到 最 终 方向 的 旋转 轴 和 旋转 角 (4 ，$)。 

3) 如 果 旋 转 占用 3s 时 间 ， 设 计 轴 角 旋 转 的 3 次 方 旋转 路 径 。 

4) 计算 该 运动 的 欧 拉 角 路 径 的 CC) 、0G) FY). 

5) 在 1) 和 3) 小 题 中 ,在 第 1 和 第 2 运动 中 计算 和 比较 wp、0、yY 的 最 大 角速度 、 最 大 
加 速度 和 最 大 加 加 速度 。 
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* 第 14 章 


时 间 最 优 控制 





工业 机 器 人 的 主要 任务 就 是 将 一 个 物体 沿 着 预定 路 线 从 静止 到 静止 的 反复 移动 。 为 了 提 
高 效率 ， 机 器 人 应 该 用 最 少 的 时 间 完 成 任务 。 我 们 介绍 一 种 求解 多 自由 度 机 器 人 时 间 最 优 控 
制 问题 的 数值 方法 。 





妈 14.1 最 优 时 间 与 bang-bang 控制 


工业 机 器 人 最 重要 的 工作 是 在 两 点 之 间 移 动 。 最 优 时 间 控 制 就 是 需要 我 们 提高 工业 机 器 
人 的 效率 。 时 间 最 优 控制 的 目的 就 是 将 机 器 人 末端 执行 器 在 最 短 的 时 间 内 从 初始 位 置 变换 到 
期 望 目标 位 置 。 用 下 面 的 运动 方程 来 研究 系统 : 
x =flx (1).Q(t) ] (14-1) 
这 里 ，Q 是 控制 输入 ,x 是 系统 状态 矢量 。 


x =- (14-2) 
q 


最 少时 间 问 题 一 直 是 有 界 输入 的 目标 问题 ， 例 如 : 
[Q (2) [KQ max (14-3) 
以 边界 输入 为 主题 的 最 优 时 间 控 制 问题 的 解 就 是 开关 控制 (bang-bang 控制 )。 将 输入 
变量 要 么 是 最 大 值 ， 要 么 是 最 小 值 的 控制 称 为 bang-bang 控制 。 
证 


最 短 时 间 控 制 的 目标 就 是 ， 在 整个 轨迹 时 间 积 分 最 小 化 的 条 件 下 ， 寻 找 从 初始 状态 zu 
G) 到 达 最 终 状 态 rra) 的 轨迹 x (1) 和 输入 Q a). 
J =| a (144) 


输入 指令 矢量 Q (1) 通常 有 式 14-3) 的 约束 。 
我 们 定义 一 个 标量 函数 及 和 一 个 矢量 P : 





H(x .Q.p)=p'f [x@).Q@)] (14-5) 
并 且 有 下 列 两 个 方程 : 
dH? 
B= (状态 方程 ) (14-6) 
Pp 
3 HT 
b=— 3G 〈 协 态 方程 ) (14-7) 


(36) 


gus nasies Gi 








>: 


依据 庞 托 里 亚 〈Pontryagin) 原理 ,最 优 输入 Q@ (1) 就 是 能 使 函数 H 极 小 值 的 输入 。 这 
样 的 一 个 最 优 输 入 就 是 在 整个 时 间 间 隔 中 产生 最 佳 效 果 ，Qamax 或 者 一 Qaax。 当 控制 指令 
区 域 边界 处 的 一 个 值 时 ， 这 被 称 为 人 饱和。 函数 H 称 为 哈密 尔 顿 (Hamiltonian) PA, KE 
P 称 为 协 态 矢量 。 

KGL 14-1 ”线性 动态 系统 。 

考虑 由 式 (14-8) 表述 的 一 个 线性 动态 系统 。 




















中 各 & 








出 




















Q=z (14-8) 
或 者 
X¥ =Ax +bQ (14-9) 
这 里 
XI o I 0 
a ae) ao 
T? 0 0 1 
对 于 输入 变量 ， 其 约束 为 
Q<l (14-11) 
定义 一 个 协 态 矢 量 
p -[ (14-12) 
p2 
则 式 (14-5) 中 的 哈密 尔 顿 函数 变 为 
H(x .Q.p)=p'(Ax +bQ) (14-13) 
式 (14-13) 可 提供 下 列 两 个 一 阶 微分 方程 : 
aHT 
x= 5 =Ax +bQ (14-14) 
op 
a. 
Wy (14-15) 
式 (14-15) 为 
A 0 
al | (14-16) 
p2 — pe 
对 式 (14-15) 积分 ， 可 得 
; C 
p js | (14-17) 
p2 = CGit tE? 
这 时 ， 哈 密 尔 顿 函数 为 
H=Qp2+pizr2=( Cit +C2)Q- Pix2 (14-18) 
控制 指令 Q 仅 出 现在 : 
p'bQ=(—Cit+C2)Q (14-19) 
Q 的 最 大 值 为 
1 —Cit+C2£0 
Q(z)= (14-20) 
一 1 —Cyt+C2<0 





该 解 表 明 , QG) 在 1 二 Cs/C1 处 有 一 个 跳跃 点 。 在 跳跃 点 处 ， 控 制 指令 突然 从 最 大 值 变 
换 到 最 小 值 ， 或 者 从 最 小 值 变换 到 最 大 值 。 该 跳跃 点 称 为 开关 点 (switching point) 。 
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将 控制 输入 式 (14-20) 代入 式 (14-9) 中 ， 则 可 得 两 个 一 阶 微分 方程 。 


HRH (14-21) 
工 ? Q 


对 式 (14-21) 积分 求 得 路 径 ra). 








1 

Wn 2 

z ET Cs) 二 Ca Q=1 
这 t+C3 

Z i (14-22) 

T2 

= aG +C 

2 3 4 OQ==1 
一 上 十 Cs 








PRY FE BL Ci. Co. C3. Cr 必须 基于 
下 列 边界 条 件 进行 计算 : 
Z0 一 Zio) Zi 一 ZCH) (14-23) 
消除 式 (14-22) 中 的 上 ， 则 可 得 到 状 
态 变量 zl Mare 之 间 的 关系 。 





1 





1 
一 误 Z22 十 C4 Q=-1 


(14-24) 

这 些 方程 表示 的 是 在 zizs* 平面 中 具 
有 参数 C4 的 一 系列 抛物 线 曲 线 。 抛 物 线 
如 图 14-la 和 图 14-1b 所 示 ， 其 中 的 箭头 
说 明 沿 着 路 径 的 运动 方向 。ziz， 平面 称 
为 相 平面 。 

考虑 该 系统 中 的 一 个 开关 点 ， 其 综 
合 最 优 路 径 如 图 14-1c 所 示 。 例 如 ， 假 
设 系 统 的 最 初 状态 和 最 终 状 态 分 别 是 
a(to) Mat). ZBAKQ=1 开始 ， 该 


eh 1 
运动 迫使 系统 按照 控制 路 径 z1 = 





























gaat 
a, ore 4 
X10 FT 移动 到 交叉 点 处 Z1 一 一 了 





1 . se he 8 
24+ (et ae]. 交叉 点 就 是 开关 点 ， 


在 该 点 处 控制 输入 变换 到 Q= 一 1。 开 关 
点 位 置 坐标 为 








1 . . 
zı 一 1 (2x10 +2x1 xo t 2) 





(14-25) 图 14-1 相 平面 中 Q= 之 的 最 优 路 径 和 最 优 路 径 的 网 格 
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1 Lg 
一 二 一 (= 一 到 (14-26) 


KG 14-2 ”机 器 人 方程 的 状态 方程 形式 。 
机 器 人 运动 方程 的 矢量 形式 为 





Dlgy +H(g,g9)1G (gq)—0 (14-27) 
我 们 可 以 定义 一 个 状态 矢量 
x -(; | (14-28) 
q 

将 运动 方程 变换 成 状态 空间 方程 

=flx(t),0()] (14-29) 
这 里 
q 
xa). Qa) j= (14-30) 
fl ace] ae Q —H -G ) 


KB 14-3 机 器 人 的 时 间 最 优 控制 。 
假设 机 器 人 的 初始 状态 为 























qo 
x(to)=5x05]| | | (14-31) 
qo 
在 最 短 的 可 能 时 间 内 ， 其 最 终 状态 为 
q; 
VL | (14-32) 
q; 
假设 在 每 个 关节 处 的 执行 器 转 矩 范围 为 
IQ; | <Q imax (14-33) 
最 优 控制 问题 就 是 最 小 化 时 间 性 能 指标 。 
J =|" d= (14-34) 
FE SL SHG BR tn eR 
H(x .Q.p)=p'f [x@).Q@)] (14-35) 
并 且 该 函数 可 提供 下 列 两 组 方程 (正则 方程 ): 
JHT 
a (14-36) 
op 
DFTT 
B= = (14-37) 





最 优 控制 输入 Q, (z) Hi EE OR HT K eME A i ES 8 TR A E BT 
时 间 最 优 问题 简化 为 两 点 的 边界 值 问题 。 边 界 条 件 是 机 器 人 在 to All tr 处 的 机 器 人 状态 。 由 
于 机 器 人 运动 方程 的 非 线 性 ， 边 界 值 问题 没有 解析 解 ， 因 此 必须 研究 数值 技术 。 

Kw 14-4 ” 欧 拉 - 拉 格 朗 日 方程 。 

为 了 说 明 路 径 z= 二 x*(1) 对 于 泛 函 是 一 个 最 小 化 路 径 。 
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其 边界 条 件 为 x (zo) 二 xo。，xz (ti) 王 xt。 我 们 需要 说 明 ， 对 于 能 够 满足 边界 条 件 的 所 有 连续 
路 径 at) 有 
JŒ] (x *) (14-39) 
PE EER rao) Sro, LONS axi WERE ce) 被 称 为 容许 路 径 。 为 了 理 
解 z* G) 是 最 优 路 径 ， 对 于 每 个 可 容许 的 路 径 我 们 可 以 检查 其 泛 函 积分。 容许 路 径 可 由 
式 (14-40) 定义 : 








X(t)=x* (t) +ey(t) (14-40) 

其 中 ， 
y(Cto) 一 y(Cr) 一 0 (14-41) 
e<] (14-42) 


ETBUR. BAZ I PAra), HAR (14-38) FAO. Z RAEI A 为 
AJ =J[la"* Heol- an|" fx * tey, * teya f" Ffr” sxe ydi 

















(14-43) 
在 (x*，z*) 处 展开 ff (a* tey, t*+ey, t) A 
. . If If 
Fæ Heyd" Heg =F at sa te(y Fy | 
Ox At 
32 a2 32 
re 人 > 5 f F2yy 元 L Hy? a z) Ole?) (14-44) 
Lj i GL G 
则 有 
AJ =eV, +e?V2+OCe?) (14-45) 
这 里 
ti 9 9 
Vi -| G f Fy a (14-46) 
to Ox IÈ 
2f Pf af 
V2 = Jx? + 2yy FEF Hy? 元 a (14-47) 


第 1 项 积分 ，Vi1i 被 称 为 泛 函 JJ 的 第 一 变 分 ; 第 2 项 积分 ，V， 被 称 为 泛 函 也 的 第 二 变 
分 。 综 合 所 有 的 高 阶 变 分 ， 表示 为 O(e3)。 如 果 x* 是 最 小 的 曲线 ， 那么 对 于 每 个 容许 的 
ya) 必须 满足 和 J 宇 0。 如 果 AS 除 以 e 并 使 :>0， 那 么 我 们 求 得 使 x* 为 最 优 路 径 Vi =0 的 


必要 条 件 。 该 条 件 等 效 于 : 
u 9 3 
| (> 1 Fy Et (14-48) 
to It 


dx 
利用 分 步 积 分 法 我 们 可 以 写 出 : 


no af L a 
| > -dt = G a AC (14-49) 


由 于 yto) =y) =0, aie (14-49) 右边 的 第 1 项 是 零 。 因 此 ， 对 于 每 个 容许 的 
y(t1)， 最 小 化 积分 条 件 式 (14-48) 简化 为 


tf 
| 
t 


af Aaaf 7 
Ee = (54) J 0 (14-50) 


方 括号 中 的 项 是 时 间 : 的 连续 函数 ， 估 计 最 优 路 径 x* ， 并 不 涉及 y(t)。 因 此 ， 乘 以 非 
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wma ye) Mrs A a A 


L SL) [RERO to e 的 定 积分 等 于 堆 的 唯一 途径 就 是 
区 
Ox dt 
st (4-51) 就 是 对 于 最 小 化 问题 式 (14-38) 的 一 个 解 x 二 x* 的 必要 条 件 。 这 个 微分 
方程 被 称 为 欧 拉 - 拉 格 朗 日 方程 。 该 欧 拉 - 拉 格 朗 日 方程 与 机 器 人 运动 方程 的 微分 方程 相同 。 
H =a 最 小 化 问题 的 第 2 个 必要 条 件 是 估计 x* 的 第 2 变 分 必须 是 负数 。 


KN 14-5 BRI 一 | 2de 的 拉 格 朗 日 方程 ， 


(14-51) 





dg 














对 于 极 值 泛 函 
2 
pa] zd (14-52) 
1 
其 拉 格 朗 日 方程 为 
9 d (ad 
of alge )--#=2 (14-53) 
式 (14-53) 表明 最 优 路 径 为 
x=Cıt +C? (14-54) 
考虑 边界 条 件 z(1)=0、z(2) 王 3， 可 得 
r=3t—3 (14-55) 
WG 14-6 ”最速 降 线 问 题 。 oA = 
1 


我 们 可 以 利用 拉 格 朗 日 方程 寻求 连接 两 点 的 无 
摩擦 曲线 ， 如 图 14-2 所 示 。 如 果 一 个 颗粒 在 重力 作 
用 下 沿 着 图 14-2 所 示 曲 线 滑 落 ， 在 最 小 时 间 段 内 从 
较 高 点 滑落 到 较 低 点 。 这 就 是 众所周知 的 最 速 降 线 






























































(brachistochrone) [al a. P, 
如 果 v 是 滑落 点 沿 着 曲线 的 速度 ,那么 滑落 弧  》 
长 ds 所 要 求 的 时 间 是 ds/v。 这 时 寻找 到 最 小 时 间 曲 图 14-2 连接 点 P1 和 Ps 的 
线 的 目标 函数 为 曲线 ， 以 及 无 摩擦 滑动 点 
J ll s (14-56) 
然而 
ds 一 V1 十 y" dx (14-57) 
依据 能 量 存储 ， 有 
vu=V/2gy (14-58) 
因此 ， 目 标 函 数 可 简化 为 
[pee (14-59) 
应 用 拉 格 明日 方程 ， 我 们 求 得 
y(1 十 y2) 一 27 (14-60) 





RE, r 是 一 个 常数 。 从 y(0) 二 0 开始 的 最 优 曲线 可 以 由 两 个 参数 方程 表述 为 
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z=r(B— sinf) (14-61) 
y=r(1—cosß) (14-62) 
最 优 曲 线 是 一 条 摆 线 。 
该 问题 的 名 称 起 源 于 希腊 语 最 短 的 和 时 间 两 个 单词 。1630 年 ， 伽 利 略 〈Galilei) 独创 
性 地 讨论 了 该 问题 ， 随 后 1969 年 约 坦 。 伯 努 利 和 和 雅 格 布 ， 伯 努 利 对 该 问题 进行 了 求解 。 
wl 14-7 拉 格 朗 日 算 子 。 
假设 在 一 个 开 区 间 内 定义 函数 f(x ) ， 并 且 该 函数 在 zo € (a,b) 的 某 一 邻 域 有 连续 的 
一 阶 和 二 阶 微分 。 如 果 式 14-63 成 立 ， 那么 点 zu 就 是 函数 f(x ) 的 一 局 部 极 值 。 
df (xo) 
dx =Q 
假设 f(x)=0, x ER” Ml g:(x)=0 G=1, 2, +, m) 是 定义 在 开 区 间 R"* 上 的 函数 ， 
并 且 在 R "空间 中 有 连续 的 一 阶 和 二 阶 微 分 。x o。 是 服从 约束 g;(x)==0 的 函数 f(x) 的 一 个 
极 值 ， 其 必要 条 件 就 是 存在 m 个 拉 格 朗 日 算 子 1 G=1, 2, ++, m), FAA 














(14-63) 
































V (s+2Aig;)=0 (14-64) 
例如 ， 我 们 可 以 求 得 下 列 函 数 的 极 小 值 。 
f=1—zx?—zx? (14-65) 
并 且 服 从 约束 : 
g=x?+x2—1=0 (14-66) 
求 得 表达 式 /十 Xg 的 梯度 为 
YL1 一 z —23 ta (23 trz: —1)]=0 (14-67) 
这 导致 
on =—221+2Ar1=0 (14-68) 
oF 2r +A =0 (14-69) 
为 了 求 得 3 个 未 知 数 zl s: MA, FAA ASK 〈14-68)、 式 (14-69) 和 式 (14-66). 
求 得 的 两 组 解 为 
x1=0 x2=1 A=2 
(14-70) 


2 一 十 1/J2 x.=1/2 A=l1 

KE 14-8 ”容许 控制 功能 。 

允许 控制 指令 Q@ (+:) 的 分 项 是 分 段 连 续 的 ， 并且 所 取 值 可 以 是 控制 空间 边界 区 域 中 的 任 
何 数 。 例 如 ， 考 虑 一 个 带 有 约束 | Q | 二 1(i= 二 1,2) 的 2 自由 度 系 统 0 (1) 一 [Q: QsJT， 控 
制 空 间 是 RQQ: 平面 内 的 一 个 圆 ， 控 制 分 项 可 以 是 圆 内 的 任何 分 段 的 连续 值 。 这 样 的 控制 被 
称 为 容许 控制 。 

例 14-9 时 间 最 优 控制 问题 的 描述 。 

最 优 时 间 控 制 的 目标 就 是 引导 机 器 人 在 最 短 的 时 间 内 沿 着 一 条 路 径 移动 ， 以 提高 机 器 人 
的 效率 。 除 了 低 阶 、 自 发 、 线 性 问题 之 外 ， 动 态 系统 时 间 最 优 控制 问题 通常 是 没有 通用 的 解 
析 解 的 。 时 间 最 优 控制 问题 就 是 一 个 有 界 输入 问题 。 如 果 对 于 一 个 给 定 的 初始 条 件 和 最 终 目 
标 存在 一 个 容许 的 时 间 最 优 控制 ， 那 么 在 任何 时 候 至 少 有 一 个 控制 变量 可 以 取得 其 最 大 值 或 
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者 最 小 值 。 根 据 庞 托 里 亚 原理 ， 具 有 边界 输入 的 最 小 化 时 间 问 题 的 解 就 是 bang-bang 控制 ， 
它 说 明了 至 少 有 一 个 输入 执行 器 在 任何 时 候 都 必须 是 饱和 的 。 然 而 ， 用 一 个 饱和 输入 信号 取 
代 另 一 个 饱和 输入 信和 号 的 开关 点 并 不 明确 ， 这 是 数值 法 所 有 解决 的 问题 。 

通常 情况 下 ， 这 个 问题 可 简化 为 一 个 很 难 求解 的 两 点 边界 值 问题 。 求 解 相 应 的 非 奇 异 、 
非 线性 两 点 边界 值 问题 ， 以 确定 开关 次 数 。 一 个 成 功 的 方法 就 是 假设 动态 系统 的 配置 轨迹 是 
可 以 预先 规划 的 ， 这 时 就 可 以 将 该 问题 简化 为 沿 着 轨迹 的 最 小 时 间 运 动 问题 。 











女 14.2 浮动 时 间 法 





考虑 如 图 14-3 所 示 的 具有 质量 m 的 物体 依据 下 列 运动 方程 正在 运动 ， 
mr=glar,¢7)+f(t) (14-71) 
这 里 的 g (a, &) 是 一 个 通用 的 非 线 性 外 力 函 数 , f (1) EAR A TA PE Hl PR PE ll 
的 边界 是 




















|f@)|<F (14-72) 
物体 从 初始 位 置 x (O=ro AA BRIS, TEAR x tp) =a, 处 停止 。 
利用 浮动 时 间 算 法 ,我 们 可 以 求解 这 个 从 静止 至 静止 的 控制 问题 ,并且 求 得 在 最 短 时 间 
内 从 xz。 到 达 z 1 移动 质量 m 所 要 求 的 函数 。 


x, 








0 Xp 


Xr—Xo 
| l 
— is =i 


图 14-3 质量 m 沿 着 一 条 直流 时 间 最 优 地 间 欧 式 运 动 














算法 14-1 浮动 时 间 技 术 

D 将 预先 规划 的 运动 路 径 a (2) 分 别 s 十 1 个 区 间 ， 并 且 确 定 所 有 坐标 值 x; (i 二 0,1,2， 
3,… ,Ss 十 1)。 

2) 设 fo 二 十 下， 计算 

















2m(x21—2x0) 
n= /二 = (14-73) 
3) Efsa F, 计算 
2m(x5—Xs—-1) 
t= ZF (14-74) 
4) 对 于 从 1 到 一 1 Wi, Ec; 以 使 f;== 十 FF。 
f; SMET g Tisti) 
4 i- i 
==) = ti b Ti- rr: p(s (14-75) 
i c=! i I= i i= 





5) WRI, ISF, WIE; GM, KA js. 
6) 计算 tj;-1， 以 使 fj; 二 一 FF。 
7) 如 果 |f;-1| 志 下， 则 停止 ,否则 设置 j= 二 j 一 1， 并 且 返 回 至 第 6) 步 。 
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证 明 : 
对 于 质点 m, 假设 g lri, či) =0 M | 
x(t), W| 14-4 PZR, 432 oh HY h E ew Be 
我 们 将 运动 路 径 分 割 成 任意 的 5 十 1， 并 且 各 OY 
段 不 必 相 等 。 因 此 ， 坐 标 x; (i 二 0,1,2,…， 
十 1) 是 已 知 的 。 浮 动 时 间 ri =t; — ti EN 


























所 要 求 的 时 间 ， 将 质点 m 从 zx; 移动 到 zi+l。 t 
利用 中 心 差分 法 ， 在 点 i 处 我 们 可 以 定义 : l - = 
LRA p — RA y i-l i i+] 
ee 图 14-4 ”质点 mm 的 运动 时 间 关系 曲线 
|, Zi} fii f 
Li sorte awa (14-76) 
=a | E |} (14-77) 
= Coty 下 十 ri 


这 些 方程 说 明 在 点 i 处 的 速度 和 加 速度 取决 于 x;、 两 个 相 邻 点 x;-1 和 zi+1 以 及 浮动 时 
间 c; 和 zi-1。 因 此 需要 在 初始 之 前 和 终点 之 后 的 两 个 附加 点 x-1 和 zz,+2 处 定义 在 x。 和 
zs+1 处 的 速度 和 加 速度 。 这 些 附加 点 及 其 相应 浮动 时 间 如 图 14-5 所 示 。 





Xo Xs+ 
i 1 
i 
i 1 
T-1 | To Ti Ts-1 Ts Ts+1 
5i to q h ts ts 人 +1 ts+2 


图 14-5 在 初始 点 之 前 和 终点 之 后 引入 的 两 个 额外 点 e M as 








在 运动 的 开始 和 结束 处 ， 其 静止 条 件 为 
并 -1 一 1 Ls+2 Xs (14-78) 
To =T-1 Tst1=Ts (14-79) 
利用 式 〈14-77) ， 广 三 mi; 在 初始 点 处 的 运动 方程 为 


4m 





fo5mto =; 7 (xı = zo) (14-80) 
To 
第 1 个 浮动 时 间 ro 的 最 小 值 由 设置 fo =F 求 得 。 
2 3 
n=] p (a1— 20) (14-81) 


它 就 是 第 1 个 浮动 时 间 的 最 小 值 ， 因 为 如 果 co 小 于 由 式 (14-81) BAMA. IBA fo 
将 大 于 下 并且 破坏 约束 式 〈14-72) 。 另 一 方面 ， 如 果 ro 大 于 由 式 (14-81) AEM. WA 
fo 将 小 于 下 并 且 输 入 仍然 不 是 饱和 的 。 相 同 的 条 件 也 存在 于 终点 ， 其 运动 方程 为 









































4 
fst+i=m Ea = (ast) (14-82) 
at 
终端 浮动 时 间 r, 的 最 小 值 可 通过 设置 f,+1 王 一 求 得 。 
2m(7xs— Xs—1) 
n= |Z — (14-83) 


544 


gus nase Gi 





为 了 求 得 ri 的 最 小 值 ， 我 们 求 得 x1 处 的 运动 方程 。 
4m To T 

De = aaa zı) ae 
这 是 一 个 具有 两 个 未 知 数 f1 和 ri 的 方程 。 通 过 调整 ri 以 使 得 fo =F. RATRE KE 
性 地 求 得 ti1。 应 用 这 个 程序 ， 通 过 应 用 最 大 力 约束 fi =F 并 且 数 值 性 地 求解 ti+1 的 运动 方 
程 ， 我 们 能 够 求 得 最 小 浮动 时 间 cio H r 是 已 知 数 并 且 当 最 大 力 应 用 于 求 得 下 一 个 浮动 
时 间 z;+1 时 ,我 们 正在 使 用 浮动 时 间 算法 的 向 前 路 径 算法 。 在 向 前 路 径 算法 的 最 后 一 步 中 ， 
在 xz,--1 处 求 得 zt,-1。 在 这 一 步 中 ， 所 有 的 浮动 时 间 rt; (i 二 0,1,2,…,s) 是 已 知 数 ,与 此 同 
时 ，f ,二 (i 二 0,1,2,…,s 一 1)， 对 于 i 二 s， 有 了 ;二 一 站。 这 时 ,在 点 x, 处 使 用 运动 方程 
而 知 的 力 F, 是 在 向 前 路 径 算法 中 唯一 不 需要 计算 的 变量 。 它 实际 上 由 点 x, 处 的 运动 方程 
可 说 明 ， 因 为 来 自问 前 路 径 过 程 的 zt,-1 是 已 知 的 ， 来自 式 (14-83) Wr, 也 是 已 知 的 以 满足 
终端 点 条 件 。 因 此 ，f, EAE i= s 处 的 运动 方程 中 可 以 求 得 ， 并 且 要 代替 tsa tss 


Xs—l1、 Xs AM x 541 o 










































































4 s— 5 
f = | oS ge =i z) (14-85) 


ctr, ts Frs tetra 

ME., WR f, ABA | (2)| 三 FF， 就 可 以 求解 ， 并且 也 可 以 确定 最 小 时 间 运 动 。 
输入 信号 fi (i 二 0,1,2,…,s 十 1), As 总 是 饱和 的 ， 任 何 一 个 浮动 时 间 也 不 再 被 减 小 。 然 
m, ANHE F, 超越 约束 | 了 (2) | 二 Ff 因为 在 带 有 f= 下 的 ;一 1 步 数 中 ， 需 要 一 个 很 大 的 动 
量 和 一 个 巨大 的 减速 加 速度 以 在 最 后 一 步 中 使 质点 m 停止 运动 。 

现在 ,我 们 逆 推 这 个 过 程 ， 使 用 向 后 路 径 算法 。 依 据 式 (14-85)， 调 整 z,-1 和 f、 以 满 
足 约束 了 ,二 一 下 。 现 在 对 于 |f(z) | 三 下 ， 必 须 检 查 f -1。 这 是 因为 在 向 前 路 径 算法 中 已 经 
ET r, ~，， 在 向 后 路 径 算法 中 已 经 求 得 了 r, -1 。 因 此 ， 六 -1 的 值 可 由 在 点 z,-1 处 的 运动 
方程 控制 。 如 果 f, 没有 超越 约束 | /GD)| 委 已 ， 就 可 以 求解 问题 ， 并 且 获 得 时 间 最 优 运动 。 
和 否则， 必须 继续 向 后 路 径 算法 ， 直 至 满足 力 约束 的 一 点 为 止 。 向 后 路 径 算法 中 ，| fe | SF 
的 位 置 zx 被 称 为 开关 点 ， 因 为 对 于 ;二 k， 0 二 j <k 时 有 FS; HF. WF GSR, R<G<st1 
时 有 f;=—F. 

KG 14-10 在 粗糙 表面 移动 一 质量 块 。 


0 2 
| | 
SEN 
S =| m 
一 = xX 


umg 


图 14-6 在 控制 力 f(z) 和 摩擦 力 pg WER FAYE EHR m 的 直线 运动 







































































如 图 14-6 所 示 ， 在 一 个 变量 力 fe) 和 摩擦 力 ymg 的 作用 下 ， 考 虑 质量 块 的 直线 运动 。 
力 的 范围 是 | f(z) | 三 FF， 其 中 士 下 就 是 力 的 界限 。 有 必要 求 得 一 个 函数 ， 它 在 最 小 的 总 时 间 
t=t) 内 移动 质量 块 mwx， 从 初始 条 件 x (0) 二 0、v (0) 二 0 到 达 最 终 条 件 x (t,) =1>0,. vO) = 
0。 该 运动 可 由 下 列 运动 方程 和 边界 条 件 所 描述 。 

f=m x — yumg (14-86) 
x(0)=0 vl0)=0 (14-87a) 
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X(t{)=lv(tt)=0 (14-87b) 
由 最 优 控制 理论 我 们 知道 ， 对 于 /=0 来 说 ， 时 间 最 优 控制 的 解 是 一 个 分 段 的 常 值 函 
数 ， 其 唯一 不 连续 处 就 在 开关 点 处 ， 此 时 :一 z 一 上/2 HA 
F t<r 
f= (14-88) 
=F t>r 

因此 ， 将 质点 m 沿 着 平滑 直线 从 x(0) 二 xo 二 0 移动 到 (zi) 二 zt 二 7 的 时 间 最 优 控制 就 
是 一 个 带 有 唯一 开关 时 间 的 bang-bang 控制 。 输 入 力 fC) 就 在 其 最 大 值 处 ， 在 t= 二 i1/2 处 。 
在 开关 点 二 (zxf 一 Xx0)/2 之 前 ， 有 JSF; 在 开关 点 xX 二 (xt 一 x0)/2 之 后 ， 有 f 二 一 了 f。 任 
何 非 对 称 特征 ， 如 摩擦 均 会 通过 移动 开关 点 将 使 问题 变 为 一 个 非 对 称 性 问题 。 

在 应 用 浮动 时 间 算 法 中 ,我 们 假设 具有 单位 质量 的 微粒 Gn = 1 kg) 在 一 个 粗糙 的 水 平 
面 上 ， 在 库仑 摩擦 (Coulomb friction〉 作 用 下 滑动 。 摩 擦 力 的 幅 值 是 umg, u 是 摩擦 因数 ， 
g 二 9. 81m/s?。 利 用 下 列 数 值 ， 我 们 应 用 浮动 时 间 算 法 。 

F=10N /二 l0m s+1=200 (14-89) 

Kl 14-7, Al 14-8 和 图 14-9 所 示 为 不 同 w 值 的 结果 。 图 14-7 说 明了 不 同 j 值 的 最 优 输 入 
力 的 时 间 关 系 曲线 。 每 条 曲线 由 jy 值 和 运动 相应 的 最 小 时 间 说 明 。 最 优 运动 x (1) 的 时 间 关 
系 曲 线 图 如 图 14-8 所 示 ， 与 此 同时 ， 最 优 输 入 f(t) 的 时 间 关 系 曲线 图 如 图 14-9 所 示 。 开 
关 时 间 和 开关 位 置 分 别 如 图 14-7 和 图 14-9 所 示 。 
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=] 
te 
tf 
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golea i ONG eres eee cee 
01 02 03 04 05 06 07 
图 14-7 不 同 摩擦 因数 w 的 最 优 输 入 fC) 的 时 间 关 系 曲 线 


























1.0F 
[ 4=0.6 
t #¢=0.591857502 
0.87 
[ u=0.4 
4 {r=0.644670867 
0.65 
$ 上 r 
0.4[ 
[ u=0.2 
0.2L 人 = 0.603536251 
| u=0 
te=0.734885 127 
下 
0.1 0.2 0.3 0.4 0.5 0.6 0.7 
图 14-8 不 同 摩擦 因数 w 的 最 优 运动 z(z) 的 时 间 关 系 曲 线 
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MWA p=0, 那么 在 运动 oF 
x(t) =1/2 的 中 点 处 并 且 到 达 
时 间 c= 2/2 的 一 半 时 开关 动 st 
作 。 运 动 总 时 间 也 随 着 x 的 增 
大 而 增加 。 















不 =0.591857502 
人 =0.603536251 
人 =0.644670867 
人 =0.734885127 












































> 一 xD) 
太 例 14-11 在 中 心 差分 
方法 中 的 一 阶 导 数 和 二 阶 
利用 泰勒 级 数 ， 我 们 在 点 
Zi1 和 点 Zi+l1 处 展开 x, FER 
一 个 点 zx; 的 外 推 。 图 14-9 不 同 摩擦 因数 y 的 最 优 力 f() 的 位 置 关 系 曲线 
Zi 十 1 一 Ti + iT; | 了 | wen (14-90) 
. l. 3 
£i- a= £Li  ÅiTi—1 g titi (14-91) 
取 上 述 表 达 式 的 前 两 项 并 且 计 算 ria WA 
a= sss (14-92) 
Ti TTi-1 
现在 ， 取 泰勒 级 数 的 前 三 项 ， 并 且 计 算 zx;41 十 x;-1， 则 有 
. _ 4m zi 一 1 “ee . | 
Ti ma a T E = (14 93) 


WG 14-12 ”收敛 性 。 

浮动 时 间 算 法 给 出 了 一 个 迭代 法 ， 因 此 必须 
辨识 收敛 准则 。 此 外 ， 必 须 定义 一 个 条 件 以 终止 
迭代 。 在 向 前 路 径 算法 中 ， 通 过 调整 浮动 时 间 r， 
到 能 够 提供 f; =F 的 一 个 值 ， 我 们 计算 浮动 时 间 
tio RIX; /I7;<0 HIX;/I7t;-1>0, HAN 
ti 收敛 于 最 小 的 可 能 值 。 图 14-10 说 明了 作为 r; 
和 zi 一 1 的 一 个 函数 去 ; 的 动作 。 利 用 式 (14-77)， 

在 收敛 的 区 域内 获得 所 要 求 的 条 件 。 
Zizi Zits Zr iO0 (14-94) 
VA eo es Ae oe ey A A) (14-95) 

















图 14-10 ”作为 rc; 和 zi 的 函数 全 ;的 行为 





b 
HE 


8(C6riri 一 1 +8r3r?_,+6r?r3_,) 








: Cr? +r?) Ci tri)? E 

ge a eee (14-97) 
Cri tri)? CG; eee” 

p> ee +r} bar iri-1 — Sei j 1 ritr) (14-98) 





(r? +r)? Ci tri)? 
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5 5 2 
8(3r? 1 一 rz 一 6r3r2 1]—3rir; 1 十 3rir4_ 1) 








Cr? +r?) Ci Hri)? A 
goa eate Iriri Deria bria) (14-100) 
5 C? +r?) Cri tri)? 

fa a a ed (14-101) 





Cel pet)? ep rei) 

收敛 条 件 确 保 之 ; 随 着 在 r; 内 的 增加 而 减少 ， 随 着 在 *;-1 内 的 增加 而 增加 。 因 此 ， 如 
有 果 固 定 r; 或 者 rz;-1， 我 们 通过 设置 f ;一 下 能 够 求 得 其 他 的 浮动 时 间 。 对 于 向 后 路 径 算 法 ， 
将 其 收敛 条 件 变 换 为 


















































Zi 二 (14-102) 
Zatsti tZ5x5 -ZLox 5-1 <0 (14-103) 
可 以 由 式 (14-104) 定义 终止 准则 。 
ll f:|—F|l<e (14-104) 
HE, e 是 一 个 用 户 预 定 的 数字 。 终 止 准 则 提供 了 一 个 好 的 方法 ， 以 确保 最 大 的 偏差 位 于 既 














定 的 边界 之 内 。 
KG 14-13 ”浮动 时 间 的 解析 计算 。 
在 一 条 直线 上 质点 m 运动 的 初始 条 件 要求 为 
£—-1 5T] (14-105) 
To =T-1 (14-106) 
在 点 zo 处， 设置 fo SF HHS EAIN E fi Smr ， 可 以 求 得 原始 的 浮动 时 间 ro 为 


Do 
2 (14-107) 
现在 ， 点 x1 处 的 运动 方程 为 


4 
fi 一 = -2 4 = £o Han) (14-108) 


一 -7 2 x 
ti tri ti +t 


将 来 自 式 (14-107) 的 ro 代入 式 (14-108) 并 且 应 用 广 = 下， 可 得 方程 为 


4mF toX2 +71 Lo j 

i“ m1 

F- = (14-109) 
2mx1—2mxo+Fr?| r14 2m an xo) 


必须 求解 式 (14-109) 才能 得 知 tio APRA (14-109) 的 rl HA xe 处 的 运动 方 
程 ， 并 且 设 置 六 = 已 ， 则 会 有 一 个 新 的 求 得 co 的 方程 。 这 个 过 程 可 以 类 似 地 被 应 用 于 其 他 
步骤 。 然 而， 以 封闭 形式 计算 浮动 时 间 并 不 是 明确 的 ， 并 且 逐 步 变 得 更 加 复杂 ， 因 此 需要 一 
个 数值 解 。 计 算 r; 的 方程 不 是 线性 的 ， 因 此 有 多 个 解 。 对 于 约束 f; 二 下 ， 必 须 检查 每 个 正 
值 解 ， 负 值 解 是 不 可 接受 的 。 

KB 14-14 最速 降 线 和 路 径 规划 。 

对 于 路 径 规 划 问 题 ， 浮 动 时 间 法 有 时 是 可 以 应 用 的 。 作 为 一 个 说 明 性 的 例子 ， 我 们 考虑 
众所周知 的 最 速 降 线 问题 。 就 像 约 萌 ， 伯 努 利 所 说 的 ,“ 一 个 物质 颗粒 沿 着 一 条 曲线 无 摩 据 
地 移动 。 这 条 曲线 将 点 A 和 点 B 连接 起 来 (点 A 置 于 点 B 之 上 )， 除 了 重力 引力 之 外 ， 没 
有 力 影 响 它 的 运动 。 从 A 到 B 的 过 程 时 间 必 须 是 最 小 的 。 这 就 会 带 来 一 个 问题 : 这 条 曲线 
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的 形式 是 什么 ?” 
最 速 降 线 问题 的 经 典 解 是 一 个 摆 线 ， 并 且 其 参数 方程 为 
X=r(B— sinf) (14-110) 
y=r(1—eosB) (14-111) 





这 里 , r 是 对 应 摆 线 的 半径 ，8 是 半径 > 的 旋转 角 。 当 8 二 0 时 ， 颗 粒 在 其 起 始点 A(0,0)。 
“4 B= 二 Bs 时 ， 颗 粒 在 第 2 个 点 B 处 。Bs 的 值 可 由 式 (14-112) 和 式 (14-113) 获得 。 


XB=r(BB— sinBs) (14-112) 
yB 一 r(1 一 cos8s ) (14-113) 

运动 的 总 时 间 为 
=e J (14-114) 


在 路 径 规划 问题 中 ， 除 了 边界 ， 
运动 路 径 是 未 知 的 。 因 此 ， 式 
(14-76) 和 式 (14-77) Pai HMA 10 
没有 给 定 。 已 知 初始 位 置 和 终止 位 
置 ， 我 们 固定 z; 坐标， 同时 保持 y: f 
坐标 自由 。 通 过 应 用 已 知 输入 力 和 寻 ot 
找 能 够 最 小 化 浮动 时 间 的 最 优 值 y;， E 
我 们 将 获得 运动 的 最 优 路 径 。 oof 

将 点 B1(l1,，0) Ml B2(2, 0) fF 上 
为 单位 质点 从 点 A(0，1) 处 掉 落 的 t 
两 个 不 同 运动 目的 地 。 图 14-11 所 示 p I 
为 对 于 s =100 通过 浮动 时 间 法 所 获 图 1411 -个 正在 掉 落 的 单位 质量 从 点 A (0,1) 到 
得 的 这 两 个 目的 地 的 运动 最 优 路 径 。 两 个 不 同 的 目的 地 的 时 间 最 优 路 多 
运动 总 时 间 分 别 是 tn = 0. 61084s, 
tn 二 0.8057s。 在 这 个 计算 中 ,重力 加 速度 在 一 Y 方向 被 假定 为 g 二 10m/s?。 

一 个 解析 解说 明 Bp, 二 1.934563rad 和 By, 一 2.554295rad。 相 应 的 总 时 间 分 别 为 tr = 
0. 6176s 和 te 二 0.8077s。 通 过 增加 ;， 所 计算 的 最 小 时 间 将 比较 接近 于 解析 结果 ， 所 估计 
的 路 径 将 比较 接近 于 一 条 摆 线 。 

最 速 降 线 的 一 个 比较 有 趣 并 且 比 较 现 实 的 问题 就 是 含有 摩擦 的 最 速 降 线 问题 和 含有 线性 
拖 搜 的 最 速 降 线 问 题 。 虽 然 对 于 这 两 种 情况 有 解析 解 ， 但 是 对 于 含有 非 线性 拖 搜 的 最 速 降 线 
问题 来 说 还 没有 任何 解析 解 。 对 于 这 种 问题 ， 应 用 浮动 时 间 法 可 以 说 是 一 个 有 趣 的 挑战 。 

















15 2.0 





























女 14.3 机 器 人 时 间 最 优 控制 











机 器 人 是 多 自由 度 的 动态 系统 。 在 具有 个 自由 度 的 机 带 人 中 ， 控 制 力 f 和 输出 位 置 x 
是 矢量 。 

f= 的 (14-115) 

X 一 (zl z2 * XT (14-116) 
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输入 力 矢 量 的 约束 为 





lf; | SF (14-117) 
XE, PSHE ER 的 分 量 可 以 是 不 同 的 。 浮 动 时 间 算 法 类 似 于 算法 14-2 而 被 应 用 ， 然 
而 ， 在 每 一 步 中 ， 力 矢量 j 中 的 所 有 元 素 对 于 其 约束 都 必须 进行 检查 。 为 了 获得 时 间 最 优 控 
H. WARES 中 至 少 有 一 个 分 量 在 每 一 步 中 必须 是 饱和 的 ， 与 此 同时 所 有 其 他 部 分 必须 
在 其 限制 范围 内 。 
算法 14-2 n 自由 度 系统 的 浮动 时 间 技 术 。 
1) 将 运动 xG) 的 预 规划 路 径 分 割 成 ;十 1 段 并 且 确 定 所 有 的 坐标 矢量 x; G=0, 1, 2, 
3, ==, stl), 
2) 在 点 xo 处 确定 运动 方程 并 且 计 算 ro 。 对 于 ro， 力 矢量 .的 唯一 一 个 分 量 在 其 较 高 
限制 处 是 饱和 的 ， 与 此 同时 所 有 其 他 的 分 量 均 在 它们 的 限制 范围 之 内 。 
fo =Fe RE 10，1，2，…，7) 
fo SF, r=0, 1, 2, 5 n rk 
3) 在 x ,+1 处 建立 运动 方程 ， 并 且 计算 r*, 。 对 于 r, 力 和 撩 量 f,41 的 唯一 一 个 分 量 在 其 较 
高 限制 处 是 饱和 的 ， 与 此 同时 所 有 其 他 的 分 量 均 在 它们 的 限制 范围 之 内 。 
fs+u =F} RE 10，1，2，…，7)} 
fs+, SF, r=0, 1, 2, «+, n rk 
4) 对 于 从 1 到 一 1 的 i 来 说 ,计算 zt; 以 使 力 拓 量 f; 中 的 唯一 一 个 分 量 在 其 较 高 限 种 
处 是 饱和 的 ， 与 此 同时 所 有 其 他 的 分 量 均 在 它们 的 限制 范围 之 内 。 
fi 5— Fe RE {0, 1, 2, *, n?) 
fi, SF, r=0, l, 2, e, n rÆk 
5) 如 果 | 了 ;| 三 F ， 则 停止 ;否则 ,设置 j) 二;。 
6) 计算 rj MEIRE S ;中 的 唯一 一 个 分 量 在 其 较 低 的 限制 处 是 饱和 的 ,与 此 同时 所 
有 其 他 的 分 量 均 在 它们 的 限制 范围 之 内 。 
D 如 果 |fj-1| 三 F ， 则 停止 ; 否则， 设置 j= 二 ;一 1 并 且 返 回 至 第 6) 步 。 
克 例 14-15 沿 着 直线 运动 的 
2R 机 械 手 。 
考虑 一 个 2R 平面 机 械 手 ， 其 末 
端点 从 静止 点 (1，1.5) 沿 着 直线 
Y=1.5 移动 到 点 (—1, 1.5) 处 停 
Jk. Al 14-12 所 示 为 一 个 带 有 刚性 机 
械 臂 的 2R 平面 机 械 手 。 该 机 械 手 有 
两 个 旋转 关节 ， 其 角 位 置 由 坐标 0 
Allo 定义 。 关 节 轴 都 平行 于 全 局 坐 
标 系 的 Z 轴 ， 机 器 人 在 平面 XY 平 
面 内 移动 。 重 力 在 一 Y 方向 作用 ， 图 14-12 带 有 刚性 机 械 臂 的 2R 平 面 机 械 手 
机 械 辟 的 长 度 是 0; 和 /1，。 
我 们 用 下 列 形式 表达 2R 机 器 人 机 械 手 的 运动 方程 。 
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P=A0+B¢+CO6¢+D¢?+M 
Q =E Ò +F ë +G?’ +N 











这 里 ，P 和 Q 是 执行 器 的 转 矩 ， 其 他 参数 为 





A=A(y)=miliTm2 i +l} +2112 cose) 
B=B(9)=m2 (13 +1112 cose) 
C=C(@)=—2mz2l,l2 sing 

D=D(@)=mz2lil2sing 

E=E(@)=B 

F=mzl3 

G=G(g)=—D 
M=M(6,9)=(m1 +mz2) gli cos? +m: gl2cos(6+@) 
N=N(,9)=m2gl2cos(O+@) 





(14-118) 
(14-119) 


(14-120) 


依据 式 (14-76) 和 式 〈14-77)， 我 们 定义 了 两 个 函数 以 离散 化 速度 和 加 速度 。 


v(t) = 


Ti+1 Li-1 


zi 十 zi 一 1 





(4 /)= 4 | Ti-] : Ti ; 
Mi oe 中 十 zi 
这 时 ， 在 每 个 瞬时 的 运动 方程 可 以 写成 


P;(t 
Q: 


这 里 ，P; 和 Q; 是 在 瞬时 i 时 所 要 求 的 执行 器 转 矩 。 假 设 执行 器 扭矩 的 范围 是 


JE 


为 了 沿 着 已 知 轨迹 时 间 最 优 地 移动 机 械 手 ， 


)=Aja (0 )+Bia(@) +C;v (6) vg) + Div? (6) +M; 








)=Eja(O + Fia(@)+Giv? C) +N; 





IP:;@)|<Pm 1Q;(t4)|<Qm 




















(14-121) 


(14-122) 


(14-123) 


(14-124) 


(14-125) 


E I YL AZ) PIT PE NY Tia] 2 HG 2G AH E 





动 时间 法 ， 运 动 在 i 二 0 RIF, BR imst+1 处 结束 。 在 ;一 一 1 和 i 一 n 十 2 处 引入 两 个 附 
加 点 ， 并 应 用 静止 边界 条 件 ， 我 们 求 得 

















To | 00 =0» 


Ts —Tst+1 0s =0s+2 Ps T Ps+2 


EBETA i 处 ， 机 械 手 的 所 有 输入 都 受 
控 于 共同 的 浮动 时 间 zr; 和 zi-1。 在 向 前 路 径 


中 ， 当 其 中 一 个 输入 








与 此 同时 其 他 的 均 在 其 限制 范围 内 ， 获 得 最 











小 时 间 ce; FE r; 时 间 





饱和 输入 ， 并 且 会 破 # 
个 约束 。 当 我 们 搜寻 到 rz 时 ， 在 向 后 路 径 





中 也 会 有 相同 的 事实 。 


P0 一 2 


(14-126) 


在 瞬时 点 i 处 饱和 时 ， 


内 ， 任 何 减 少 都 会 增加 
坏 式 (14-125) 中 的 一 





a E E E E E e E E 








L 1 L 
“to =0.5 0 0.5 


pt 
1.0 X 


考虑 下 列 数 值 和 图 14-13 所 示 的 运动 ”图 14-13 在 直线 Y=1.5 上 从 点 G. 1.5) 到 


(一 1，1.5) 移动 的 2R 平面 机 械 手 
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mı =m =lkg /1 三 /2 一 1m 
Pm=Qm=100N * m (14-127) 

H TMATE A, 7] FL a 100 
向 运动 学 ， 第 卡 儿 空间 中 的 运动 路 
径 首 先 必须 被 转换 到 关节 空间 中 。 
这 时 ， 关 节 空 间 中 的 运动 路 径 必 须 
离散 化 成 任意 段 ， 例 如 200 段 ， 应 
该 应 用 算法 14-2。 

图 14-14 所 示 为 在 应 用 浮动 时 
间 算 法 后 具有 最 优 时 间 运 动 的 执行 -50 上 
器 转 矩 。 在 这 个 过 程 中 ， 存 在 着 一 5 
个 开关 点 ， 即 固定 执行 器 的 转 矩 从 -100 
最 大 值 转换 到 最 小 值 。 未 固定 的 执 
行 器 转 抢 不 会 人 饱和， 但 是 我 们 期 望 
其 中 的 一 个 输入 总 是 饱和 的 。 在 关 
节 坐 标 空间 中 ， 计 算 浮 动 时 间 人 允许 我 们 计算 运动 信息 。 在 稍 卡 儿 坐 标 系 中 ， 利 用 关节 坐标 时 
间 变 化 曲线 可 以 确定 末端 执行 器 的 运动 学 。 

Kl 14-16 ”多 开关 点 。 

制造 如 图 14-12 所 示 的 2R 机 械 手 服 








>y 





转 矩 /Nm 














0 l 0.098 ' 0.197 0.296 0.395 
图 14-14 2R 机 械 手 沿 着 直线 Y 一 1.5、 
—1I<X <1 的 时 间 最 优 控制 




















































































































从 图 14.15 中 所 示 的 路 径 。 浮 动 时 间 算法 
按照 下 列 参数 运行 。 
mı =m =lkg li=l:=lm 0.4 
Pm=Qm=100N = m 0.2 
X(0)=1.9m X(t11)=0.5m Y=0 i ariii M h \ sy 
0 02 04 06 08 10 #12 414 16 1.8 
(14-128) 图 1415 2R 平面 机 械 手 沿 直 线 y=0 的 运动 说 明 


利用 这 些 参数 会 得 出 如 图 14-16 所 示 
的 解 。 在 图 14-16 中 有 3 个 开关 点 。 显 然 ， 固 定 执 行 器 是 饱和 的 ， 并 且 未 固定 的 执行 器 在 其 











100 





-= 


-75 

















—100 





1 1 1 
0 0.079 0.157 0.236 0.315 


Al 14-16 2R PLR IGA HA Y=0. 0.5<X <1. 9 的 时 间 最 优 控制 输入 
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限制 范围 内 需要 一 个 正 的 转移 。 在 第 1 个 开关 点 ， 未 固定 的 执行 右 到 达 它 的 负极 限 。 固 定 的 
执行 器 展示 了 从 正 到 负 的 变化 ， 直 到 当 第 2 个 开关 点 出 现时 它 到 达 负 极限 为 止 。 在 第 2 个 开 
关 点 和 第 3 个 开关 点 之 间 ， 固 定 执行 器 是 饱和 的 。 最 后 ， 对 于 第 2 个 时 间 ， 当 未 固定 的 执行 
器 到 达 其 负极 限时 ， 第 3 个 开 点 出 现 。 























14.4 Aw pak 





EKME, FEA VAT a AE VA ie EE — — 4g FER DH A ik HE H AD BR eh TE 
时 ,我 们 称 其 为 饱和 。 一 个 n 自由 度 机 器 人 的 时 间 最 优 控制 有 一 个 简单 的 解 : 在 每 一 个 瞬 
时 ， 至 少 有 一 个 执行 器 必须 是 饱和 的 ， 与 此 同时 其 他 的 执行 器 必须 位 于 其 界限 内 。 浮 动 时 间 
法 是 一 个 用 于 寻求 饱和 执行 器 、 开 关 点 和 非 饱 和 执行 器 输出 的 应 用 性 方法 。 开 关 点 就 是 饱和 
执行 器 在 非 此 即 彼 之 间 开 关 的 点 。 

浮动 时 间 法 是 以 离散 运动 方程 为 基础 ， 利 用 变 分 时 间 增 量 的 方法 。 然 后 ， 依 据 迭 代 算 
法 ,计算 机 器 人 执行 器 所 要 求 的 输出 ， 以 使 其 服从 一 个 给 定 的 运动 路 径 。 

















习题 


14-1 标记 与 符号 。 
描述 下 列 这 些 符号 的 含义 。 
1) to 2) ti 3) fi 4) x; 5) ae 








6) a; 7) t-1 8) Ts 9) zs+1 10)F, 
K14-2 2 自由 度 系统 的 时 间 最 优 控制 。 
考虑 一 个 动态 系统 
X11 二 一 3X1 十 2X2 十 5Q 
Z2 一 27z1 一 37， 





具有 边界 控制 输入 |Q S. 的 该 系统 必须 从 任意 初始 条 件 开 始 ， 结 束 于 zi 一 z2? 一 0。 
证 明 如 下 函数 





三 三 一 3z1 十 2z? 十 5Q 
f2=—2241—32x2 
Js=] 


连同 下 列 哈 密 尔 顿 函数 以 及 协同 变量 pa 和 可 以 解决 这 个 问题 。 
H=—1+ pi(—321 +222 +5Q)+ p2 (221-322) 

W14-3 JPACPE H PR ewe. 

考虑 一 个 一 维 控制 问题 








%=—xt+Q 
这 里 ，Q 是 控制 指令 。 变 量 z 二 x (1) 必须 满足 下 列 边界 条 件 : 
x(0)=a.x;) =b 











必须 使 目标 函数 /最 小 化 。 
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证 明 如 下 函数 


f3=0 
¥ [a] PF BS OF OK RA A Re AB ee po 和 pi 可 以 解决 这 个 问题 。 





1 
H=~ po 2+ pi x+Q) 


友 14-4 对 原点 的 时 间 最 优 控制 。 
考虑 如 下 一 个 动态 系统 


zs 二 一 X11 十 Q 
具有 边界 控制 输入 1Q | <1 的 该 系统 必须 从 任意 初始 条 件 开始 ， 结 束 于 相 平 面 原点 ， 
2Z1 一 Z2 一 0。 试 求 以 最 少时 间 完 成 这 个 运动 的 控制 指令 。 

















W145 线性 动态 系统 。 
考虑 一 线性 动态 系统 
XZ =Ax +bQ 
这 里 


该 系统 服从 于 对 控制 指令 的 边界 约束 。 
QE1 
试 求 将 系统 从 xo 移 动 到 xi 的 时 间 最 优 控制 指令 Q。 


=] 1 
1) xo ， 光 1 一 o 
一 下 íl 
=] 3 
2) X= > X15 š 
一 让 1 


14-6 ”约束 最 小 化 。 
试 求 服从 于 下 列 约束 的 函数 的 局 部 极 小 值 和 局 部 极 大 值 。 
f(z)=zxi 二 zi 二 Tx? 








LD gi=azi1txze+2z3—3=0,. 
2) g1 二 zx? 十 x2 十 Xi 一 5 二 0，g2 二 Zz 
友 14-7 具有 不 同 限制 的 控制 指令 。 
光滑 平面 上 有 一 个 控制 力 FGz) ， 在 该 力 影 响 下 考虑 m 一 1kg 质点 的 直线 运动 。 该 力 受 
限于 六 三 f (2) 三 FF，。 假 设 质 点 以 最 小 的 总 时 间 :一 二 从 初始 条 件 x (0) =0, v (0) = 0% BHR 
最 终 条 件 (11) 一 10m、w(t1) 二 0。 对 于 下 列 控制 力 ， 利 用 浮动 时 间 法 求 得 所 要 求 的 控制 指 
令 (4) 二 mz 和 开关 时 间 。 














2 2 2 
1 十 ZX; 十 X33 一 2X1 一 3 二 0。 
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1) Fi=10N, F2=10N, 

2) Fi=8N, F:=10N. 

3) F,;=10N, F2=8N, 

Ww14-8 具有 不 同 限制 的 控制 指令 。 

试 求 将 质量 闷 王 2kg 的 质量 块 从 Pi 处 静止 开始 移动 到 P*， 然 后 返回 并 停止 在 P; 处 ， 
如 图 14-17 所 示 。/ 值 如 下 : 

1) u50. 

2) p=0. 2. 




















Ps 


图 14-17 从 Pi 人 处 静止 开始 移动 到 了,， 然 后 返回 并 停止 在 P 处 的 质量 块 直线 运动 























克 14-9 阻抗 性 介质 。 

考虑 图 14-17 中 m= 2kg 的 质量 块 ， 假 设 质量 块 以 最 少时 间 以 从 静止 到 静止 的 方式 从 
Pi 移动 到 P;。 控 制 指令 受 限于 | f(z) | 三 20N。 然 而 ， 具 有 与 速度 cx 呈正 比例 的 空气 阻抗 
特性 。 确 定 如 下 参数 条 件 下 的 最 优 函 数 f (7) 。 

1) y=0, c=0.1。 

2) /一 0.2，c 一 0. 1, 

妈 14-10 ”在 摩擦 和 弹 得 力作 用 下 的 质量 块 运动 。 

试 求 将 m= 二 1kg 的 质量 块 以 从 静止 到 静止 方式 从 x (0) 二 0 移动 到 (11) 二 10m 的 最 优 控 
HSS | fC) | 三 100N。 质 量 块 在 摩擦 因数 y 的 粗糙 表面 上 移动 ， 并 且 通 过 刚度 为 & 的 线性 
































弹簧 安装 固定 到 一 堵 墙 上 ， 如 图 14-18 R. u, k 的 值 如 下 : 
1) #=0.1, k=2N/m, 
2) #=0.5, k=5N/m, 














Umg 


图 14-18 位 于 粗糙 表面 且 用 弹簧 附加 到 墙 上 的 质量 块 的 直线 运动 





























KW14-11 ”收敛 条 件 。 

针对 浮动 时 间 算 法 的 收敛 条 件 ， 请 验证 式 (14-96) 一 式 (14-101)。 

W14-12 沿 着 直线 或 圆 运 动 的 2R 机 械 手 。 

对 于 如 图 14-19 的 2R 机 械 手 ， 请 计算 其 驱动 力矩 ， 以 使 端点 以 时 间 最 优 方式 从 Pi 
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(1.5m, 0.5m) 移动 到 P，(0，0. 5m)。 该 机 械 手 具有 如 下 参数 : 
mı =m2z=lkg 1=/l;=lm 
|P@)|<100N |Q(t)|<80N+m 
运动 路 径 如 下 : 
1) 一 条 直线 。 
2) 圆心 位 于 (0.75m，0.5m) 处 的 一 个 半圆 。 
友 14-13 极 坐标 机 械 手 的 时 间 最 优 控制 。 
14-20 所 示 为 一 个 极 坐 标 机 械 手 ,该 机 械 手 由 力矩 @ 和 力 P 所 控制 。 基 地 驱动 器 转动 
HRF. HP 使 第 2 根 连 杆 在 第 1 根 连 杆 上 滑动 。 对 于 下 列 数据 ， 试 求 将 端点 从 Pid. 5m， 
lm) 移动 到 P:(—1m, 0.5m) 的 最 优 控制 。 
m,=5kg m2 一 3kg 
IP@)|<80N |Q(t)|<100N+m 


= 





= 











=X 








下 





图 14-20 WHE Q 和 力 P 所 控制 的 极 坐标 机 械 手 


= 


414-140 关节 式 机 械 手 的 控制 





假设 连 杆 是 由 均匀 杆 所 构成 的 ， 试 求 如 图 5-22 所 示 关 节 式 机 械 手 沿 着 直线 从 Pi 一 
(1.1, 0.8, 0.5) BaF P2=(—1. 1, 0.35) 的 时 间 最 优 控 制 。 机 械 手 的 几何 参数 如 下 : 








dl 一 1m d2=0 
/2 一 1m /3 一 1m 
mı =25kg m2=12kg m3 一 8kg 


1Q,:()|<180N*m_ |Q2(t)|<100N*m_ |Q3(¢4)|<50N+m 
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对 于 机 器 人 末端 执行 器 所 期 望 的 几何 路 径 ， 可 以 利用 逆向 运动 学 计算 关节 和 运动。 将 关节 
运动 代入 运动 方程 则 可 知 执行 器 的 指令 。 应 用 所 得 指令 将 使 机 器 人 沿 着 所 期 望 的 路 径 理 想 地 
移动 末端 执行 器 。 然 而 ， 因 为 存在 干扰 和 未 建 模 现 象 ， 因 此 机 器 人 将 不 会 按照 期 望 的 路 径 移 
动 ， 使 偏差 最 小 化 或 者 消除 的 技术 则 被 称 为 控制 技术 。 


15.1 开 环 控制 和 闭环 控制 


机 器 人 在 每 个 关节 处 均 有 一 个 执行 器 的 机 械 机 构 ， 该 执行 器 用 一 个 力 或 力矩 驱动 连 杆 
(i)。 机 器 人 用 位 置 传感器 、 速 度 传感器 以 及 可 能 的 加 速度 传 感 右 测量 关节 变量 的 运动 。 所 
测量 的 值 通 常 是 附着 在 连 杆 (i) 上 且 相 对 于 坐标 系 Bi;-1 或 者 Bo 的 坐标 系 B: 的 运动 信息 。 

为 了 使 机 器 人 的 每 个 关节 都 能 服从 期 望 的 运动 ， 我 们 必须 提供 所 要 求 的 转 矩 。 假 设 已 知 
的 关节 变量 gd=g (2) 的 期 望 路 径 是 时 间 的 函数 ， 那 么 使 机 器 人 服从 期 望 的 运动 所 要 求 的 转 
矩 可 通过 式 (15-1) 计算 。 

Q.=D(qa)qa HH (qaqa) HG (qu) (15-1) 

这 里 ， 下 标 d 和 ec 分别 代表 预期 (desired) 和 控制 (controlled)，。 

理想 世界 中 可 以 确切 测量 变量 ， 并 且 机 器 人 基于 式 (15-1) 能 够 完美 地 工作 。 执 行 器 控 
制 转 和 矩 Q@. 可 以 产生 期 望 路 径 qa 。 这 就 是 开 环 控制 算法 ， 即 基于 一 个 已 知 的 期 望 路 径 和 运动 
方程 计算 控制 指令 ， 然 后 控制 指令 作用 于 系统 则 会 产生 所 期 望 的 路 径 。 因 此 ， 在 一 个 开 环 控 
制 算法 中 ， 我 们 希望 机 器 人 能 够 服从 所 设计 的 路 径 ， 然 而 没有 任何 机 械 机 构 可 以 补偿 所 有 可 
能 的 误差 。 

现在 假设 在 机 器 人 运动 期 间 通 过 测量 关节 运动 ， 我 们 可 以 观察 机 器 人 。 在 任何 瞬时 在 实 
际 关节 变量 和 期 望 值 之 间 总 有 一 差 值 。 这 个 差 值 称 为 偏差 .可 由 式 (15-2) ASR (15-3) 
测 得 : 

















e =q 4a (15-2) 
e =q 一 ga (15-3) 

现在 让 我 们 定义 一 个 控制 规律 ， 并 且 通 过 式 (15-4) 计算 一 个 新 的 控制 转 矩 矢量 
Q =Q.+kpe +kpe (15-4) 


这 里 ，kp 和 kp 是 控制 增益 常量 。 控 制 规律 将 实际 关节 变量 aqo qg) SWE a 
qa) 相 比较 ,成 比例 地 产生 一 个 控制 指令 。 应 用 新 的 控制 指令 改变 机 器 人 动态 方程 以 产生 
实际 关节 变量 g 。 
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Q.+kpe +kpe =D(q)q +H(g .q)+Gq) (15-5) 
15-1 以 功能 图 的 形式 说 明了 这 种 控制 方法 的 主要 含义 ， 这 就 是 闭环 控制 算法 。 在 该 
控制 算法 中 基于 实际 值 和 期 望 值 之 间 的 偏差 计算 控制 指令 。 读 取 实 际 变 量 并 且 将 其 与 期 望 值 
比较 ， 称 其 为 反馈 。 正 因为 如 此 ， 闭 环 控制 算法 也 可 称 为 反馈 控制 算法 。 
o, 


q 
控制 器 | 一 一 (+) 一 | 动力 学 响应 | 一 


图 15-1 反馈 控制 算法 示意 图 


控制 句 提 供与 误差 及 其 时 间 率 成 比例 的 信号 。 该 信号 与 预期 指令 @. 相 加 以 补偿 偏差 。 
反馈 控制 原理 可 以 表述 为 ， 当 实际 变量 值 小 于 期 望 值 时 增 大 控制 ， 当 实际 变量 值 大 于 期 
望 值 时 减 小 控制 。 
例 15-1 质量 弹簧 阻尼 振荡 需 。 
考虑 一 个 线性 振荡 器 ， 它 由 质量 - 弹 移 -阻尼 顺 
系统 所 构成 ， 如 图 15-2 所 示 。 在 外 部 作用 力 了 E 
用 下 ,振荡 器 的 运动 方程 为 
mater +kr=f (15-6) 
这 里 ， 外 部 作用 力 f 是 控制 指令 ，m 是 振荡 物体 
的 质量 ，c 是 黏 性 阻尼 , k 是 弹 得 刚度 。 获 得 期 望 
位 移 x4 二 x(t) 所 要 求 的 作用 力 可 由 运动 方程 计算 所 得 。 
fe=m ty tex, t kra (15-7) 































































































= 














图 15-2 ”线性 质量 阻尼 振荡 器 














开 环 控制 算法 如 图 15-3a 所 示 。 








太一 一 一 | 动力 学 响应 [一 一 ~ xa 











a) 


f, 
Xa e IA f = p 
a 控制 器 动力 学 响应 


b) 








Ys 
































x4 g f 


控制 器 oo 动力 学 响应 =x 


























c) 
图 15-3 2R PEDR W Ai Fe IN PE ll FE IA A A IE PE h BEI 
a) 开 环 控制 算法 b) 新 控制 算法 c) 调整 后 的 算法 
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为 了 消除 所 有 可 能 的 误差 ， 我们 可 以 使 用 期 望 输出 与 实际 输出 之 间 的 偏差 e 王 过 一 zd， 

并 且 定 义 一 个 控制 规律 。 
f=fetkpetkpe kp>0 kp>0 (15-8) 
新 的 控制 规律 使 用 如 图 15-3b 中 所 示 的 反馈 指令 。 





























也 有 可 能 仅仅 只 是 基于 偏差 信号 便 可 定义 一 个 新 的 控制 规律 ， 例 如 : 
f=— kpe —kpe (15-9) 
利用 这 个 规律 ， 我 们 可 以 定义 一 个 更 加 简洁 反馈 控制 算法 ， 将 运动 方程 调整 为 
mat(ctkpxt(khtkp)x=kp my thkpxra (15-10) 


系统 方程 可 被 总 结 为 如 图 15-3c 所 示 的 功能 图 。 

可 以 解释 反馈 控制 系统 的 常规 原理 ， 以 便 将 来 自 输出 信号 反馈 回 至 输入 端 ， 并 与 其 比 
较 。 这 个 反馈 信号 形成 了 一 个 环 ， 它 使 得 使 用 反馈 〈feedback) 和 闭环 (close-loop) 变 得 合 
理 。 闭 环 控制 的 原理 就 是 检测 在 实际 输出 值 与 期 望 输出 值 之 间 的 任何 偏差 。 只 要 偏差 信号 不 
为 零 ， 控 制 器 继续 改变 控制 指令 以 便 使 偏差 信号 收敛 于 零 。 

例 15-2 受 控 系 统 的 稳定 性 。 

考虑 一 个 线性 质量 - 弹 和 佬 -阻尼 振荡 器 如 图 15-2 所 示 ， 其 运动 方程 如 下 : 



































matca tkr=f (15-11) 
我 们 定义 一 个 基于 实际 输出 值 的 控制 规律 : 
太一 一 ADZ 一 Apz kp>0 kp>0 (15-12) 
于 是 有 
mat letkp)xt(k+kp)x=0 (15-13) 





与 开 环 方程 [ 式 (15-11) ] 相 比 ， 闭 环 方程 [ 式 (15-13) ] 表明 振荡 器 在 新 的 刚度 Ck 
+kp) 和 阻尼 (c 十 kp) 作用 下 充当 着 一 个 自由 振荡 系统 。 因 此 ， 控 制 规律 已 经 明显 地 改变 
了 实际 系统 的 刚度 和 阻尼 。 这 个 例子 介绍 了 控制 理论 最 基本 的 应 用 ， 以 提高 系统 的 特性 并 且 
使 系统 按照 期 望 的 方式 动作 。 

当 系 统 的 期 望 输出 改变 时 ， 控 制 系统 必须 是 稳定 的 ， 并 有 旦 也 能 够 消除 扰动 的 影响 。 控 种 
系统 稳定 性 定义 为 : 对 于 给 定 的 输入 或 者 有 界限 的 干扰 函数 ， 输 出 必须 是 有 限 波动 的 。 

为 了 探讨 系统 稳定 性 ， 我 们 必须 求解 闭环 方程 [ 式 (15-13)]。 该 方程 是 线性 的 ， 因 此 
它 有 一 个 指数 解 。 
































= 






































X=e* (15-14) 
将 其 代入 式 (15-13) 中 ,得 其 特征 方程 为 
ma2+(ct+tkp)At(khtkp) =0 (15-15) 
它 的 两 个 解 是 
-+k Fep)? —4m(k Fk 
ii c Dy (c+kp) m(k+kp) (15-16) 
2m 2m 
fe [sb (15-14) ] 本 质 上 取决 于 A1、X4，。、 以 及 kp 和 kp。 该 特征 方程 的 根 是 复数 。 
A= 一 a 土 bi (15-17) 
ctkp 
a= (15-18) 
2m 
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Am(k+kp)—(c+kp)? 








= (15-19) 
假设 增益 为 
(ctkp)?<4m(k+kp) (15-20) 
在 这 种 情况 下 ， 运 动 方程 的 解 为 
x=Ce 0 sin(w, (I-22 t+¢@) (15-21) 
这 里 
ETE 
On= TE 到 (15-22) 
+k 
SS (15-23) 








7 m(k+kp) a? +b? 
参数 wn 被 称 为 固有 频率 ，& 是 系统 阻尼 比 。 aaa 阻尼 比 控制 着 系统 行为 : 
1) WR E=0, WA He AE (He AH PA. A12 = Ebi tiz /ath FR) 这 种 情况 


下 ， 系 统 没 有 阻尼 ， ER one 工 一 0。 
2) 如 果 0<é 二 1， 那 么 该 系统 是 欠 阻 尼 的 ， 它 绕 着 平衡 点 振动 ， 并 且 幅 值 逐 渐 减 小 。 
在 这 种 情况 下 ， as dena 的 。 





















































3) WR =1， 那 么 系统 是 临界 阻尼 的 。 临 界 阻 尼 振 水 器 能 以 非 振 荡 的 方式 用 最 快 的 速 
ese 








4) MWR ESL, PARSE MIEN. IF AE SAE Ge HA WY rh ee Hh ak E OF ie 
置 。 其 特征 值 是 实数 ， 过 阻尼 振荡 器 的 解 为 








x=Ae™! + Be?! (A12 ER) (15-24) 
5) 如 果 & 二 0， 那 么 系统 是 不 稳定 的 ， 因 为 其 解 是 
x=Ae*!+Ber?* Re(Ai,2) 0 (15-25) 


式 (15-25) 说 明 系 统 的 运动 是 一 个 幅 值 不 断 增 大 的 运动 。 
KB 15-3 ”特征 方程 的 解 。 
考虑 具有 下 列 特征 方程 的 一 个 系统 : 
42+6A+10=0 (15-26) 
该 方程 的 解 为 
)1.2 一 一 3 士 i (15-27) 
w (15-27) 说 明 该 系统 是 一 个 稳定 系统 ， 因 为 Re(ai.2)=—3<0, 
特征 方程 是 线性 多 项 式 ， 因 此 有 可 能 用 数值 法 ， 如 牛顿 -拉夫 和 森 法 求解 和 确定 系统 的 稳 
定性 。 
* Gl 15-4 复数 根 。 
如 果 特 征 方程 复数 根 为 














à1,2 =a Ebi (15-28) 
那么 我 们 可 以 利用 欧 拉 公 式 
e” =cos6 + isin (15-29) 
这 说 明 ， 解 可 以 被 写 为 
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x =C e” (cosbt +isinbt) +C2e (cosbt —isinbt) =e (Acosbt+Bsinbt) (15-30) 

这 里 ，C1 MC. 是 复数 ，A 和 B 是 实数 ， 其 关系 为 
A=C,+C, (15-31) 
B=(Cı—C2)i (15-32) 





例 15-5 机 器 人 控制 算法 。 

机 器 人 采用 的 是 非 线 性 动态 系统 ， 在 每 项 任务 中 没有 一 个 通用 的 方法 用 于 设计 适应 于 每 
个 机 器 人 的 非 线 性 控制 器 。 然 而 ， 有 多 种 可 选择 并 且 相 互补 充 的 方法 ， 对 于 特殊 的 任务 每 种 
方法 都 有 其 所 应 用 的 特定 机 器 人 种 类 。 最 重要 的 控制 方法 如 下 : 

1) 反馈 线性 化 或 者 计算 机 转 矩 控制 技术 。 在 反馈 线性 化 技术 中 ， 我 们 定义 控制 规律 以 
获得 一 个 用 于 侦 差 指令 的 线性 微分 方程 ， 然 后 再 使 用 线性 控制 设计 技术 。 虽 然 反 馈线 性 化 技 
术 已 成 功 地 应 用 于 机 器 人 ， 然 而 根据 参数 不 确定 性 或 干扰 它 并 不 能 保证 鲁 棒 性 。 

这 种 技术 是 一 种 基于 模型 的 控制 方法 ， 因 为 所 设计 的 控制 规律 是 建立 在 机 器 人 标 称 模型 
的 基础 之 上 的 。 

2) 线性 控制 技术 。 控 制 机 器 人 最 简单 的 技术 就 是 基于 操作 点 运动 方程 线性 化 而 设计 一 
个 线性 控制 器 。 线 性 化 技术 局 部 地 决定 机 器 人 的 稳定 性 。 比 例 、 积 分 、 微 分 或 者 它们 的 任何 
综合 都 是 最 常用 的 线性 控制 技术 。 

D 自 适 应 控制 技术 。 自 适应 控制 是 一 种 用 于 控制 不 确定 性 或 者 时 变 机 器 人 的 技术 。 自 
适应 控制 技术 对 于 低 自 由 度 机 器 人 比较 有 效 。 

A) 和 鲁 棒 自 适应 控制 技术 。 重 棒 控 制 方法 是 基于 标 称 模型 加 一 些 不 确定 性 而 设计 控制 器 
的 。 不 确定 性 可 以 位 于 任何 参数 中 ， 如 末端 执行 器 承载 的 载荷 。 例 如 ， 对 于 范围 在 1 一 10kg 
的 载 集 而 言 ， 我 们 研制 这 种 控制 技术 是 有 效 的 。 

5) 增益 调度 控制 技术 。 增 益 调度 是 一 种 将 线性 控制 技术 应 用 于 机 器 人 非 线 性 动力 学 之 
中 的 技术 。 在 增益 调度 中 ， 我 们 选择 大 量 的 控制 点 以 覆盖 机 器 人 操作 范围 。 这 时 在 每 个 控 囊 
点 ， 我 们 对 于 机 絮 人 动力 学 进行 线性 时 变 近似 ， 并 且 设 计 一 个 线性 控制 器 ， 然 后 在 控制 点 之 
间 捅 入 或 者 调度 控制 器 参数 。 














































































































= 








15.2 ieee mee hl 


机 器 人 动力 学 可 以 被 表述 成 下 列 形式 
Q =D4 )g +H(q ,gq )+G (q) (15-33) 
这 里 ，g 是 关节 变量 矢量 , QO (gq , gq. O 是 应 用 于 关节 处 的 转 矩 。 假 设 在 关节 空间 中 ， 期 
望 路 径 由 二 次 微分 函数 gq ae 给 定 。 因 此 ， 关 节 位 置 的 期 望 时 间 历 史 、 关 节 速 度 和 
关节 加 速度 是 已 知 的 。 我 们 通过 引入 下 列 的 计算 转 矩 控制 规律 可 以 控制 机 器 人 使 其 服从 期 望 
路 径 。 















































Q =D(q)(qa—kpe —kpe)+H(q ,gq )+G (q) (15-34) 





这 里 ， e 是 偏差 矢量 里 : 





e =q 一 ga (15-35) 
kp 和 kp AIM aT AREE, REP EY RP IE A ARK. PE ll UL Ek fa EY 
也 是 可 被 应 用 的 。 
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0 I 
A=| | (15-36) 
—kp —kp 
证 明 : eRe Ase ae. ATW A BOR IB gy (2) 所 要 求 的 Q.。 
Q.=D (qadda tH (qasqa) HG (qu) (15-37) 
PITIT A Be FON Pe hl a A. PE HI FESE FIP PE IPE. CE — PF HIP. RTA BL 
an A WZATI. J H m BE OT BER AY Pe FB EL AB PK RA EE. FFE il E— +} GA 





WETE. A BLA AY 4 IR ASSES A Pa A. 
由 于 未 建 模 参数 和 在 调整 中 存在 的 误差 ， 因 此 期 望 路 径 和 理想 路 径 之 间 总 会 有 一 个 差 
值 。 为 了 使 机 器 人 的 实际 路 径 趋 于 理想 路 径 ， 我 们 必须 引入 一 个 反馈 控制 。 让 我 们 使 用 实际 
路 径 中 的 反馈 信号 ， 并 且 将 计算 转 矩 控制 规律 [ 式 5-3] 应 用 于 机 器 人 之 中 。 替 代 运 动 
方程 [ 式 (15-33) ] 中 的 控制 规律 ， 则 有 

e +kpe +kpe =0 (15-38) 
对 于 在 实际 输出 与 期 望 输出 之 间 的 误差 变量 来 说 ， 这 是 一 个 线性 微分 方程 。 如 果 假 设 n Xn 
增益 矩阵 ky 和 kb 是 对 角 线 矩阵 ， 那 么 我 们 可 以 矩阵 形式 重新 书写 误差 方程 。 















































ei el 
| 二 |= . |=Al. (15-39) 
di le kp —~kp }le e 
当 和 矩阵 A 的 所 有 特征 值 均 有 负 实 部 时 ， 式 (15-39) 是 渐 近 稳定 的 。 和 矩阵 kp 起 着 固有 频率 的 
EH, kp 充当 着 阻尼 作用 。 
w] 0 0 
0 w? tee? AO) 
kp=| . ss : (15-40) 
0 0 w? 
2610 0 0 
kp= . (15-41) 
0 0 se 2Enw, 





TON kpl kp xe YT HRE, BERNIA A ad ee a RE Kp 和 jp 以 便 独 立地 控制 机 器 人 在 
每 个 关节 处 的 响应 速度 。 对 于 和 矩阵 来 说 ， 一 个 简单 的 选择 就 是 设计 和 全 =0 G=1, 2, -, 
n) 并 且 使 每 个 关节 响应 等 于 一 个 具有 固有 频率 w; 的 临界 阻尼 线性 二 阶 系统 的 响应 。 
计算 转 矩 控制 规律 ， 式 (15-34) 有 如 下 两 个 部 分 。 
Q =D(q)qu+H(q -q )+G (q)+D(q)(—kpe —kpe) 














Qi Q r (15-42) 

第 1 UWQ-- RATES. FEF IPM PE HEE EE A OT BER Pe, “PAIR ZEIT. PE h TAA 

Qy/ 使 得 机 器 人 服从 期 望 的 路 径 ga; 第 2 项 Qj, 是 反馈 指令 ， 它 就 是 减少 机 器 人 路 径 中 误差 
的 修正 转 矩 。 

计算 转 矩 控制 也 被 称 为 反馈 线性 化 ， 对 于 机 器 人 非 线性 控制 设计 来 说 它 是 一 个 应 用 技 

术 。 为 了 应 用 反馈 线性 化 技术 ， 我 们 开发 控制 规律 ， 以 便 消除 所 有 非 线性 ， 减 少 误差 信号 的 
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线性 二 阶 方程 问题 。 
例 15-6 振荡 器 的 计算 力 控 制 。 
15-2 所 示 为 控制 力 的 作用 下 的 一 个 线性 质量 - 弹 自 -阻尼 振荡 器 。 振 荡 器 的 运动 方程 为 











mate rtkr=f (15-43) 
应 用 一 个 计算 力 控 制 规律 
f=m(a—kp e —kpe)+c a tkz (15-44) 
e=x-2a4 (15-45) 
将 偏差 微分 方程 简化 为 
e 十 &De +kpe=0 (15-46) 
ind 2E DT FE BY fE AI 
e = Aet + Be?! (15-47) 
à1.2=— kpn E /kh—4kp (15-48) 
这 里 ，A MB 是 初始 条 件 的 函数 ，41, 是 特征 方程 的 解 。 
ma? +kpatkp=0 (15-49) 





WR kp >O, fifi (15-47) 是 稳定 的 ， 随 着 上 ~co，e 指数 地 趋 于 零 。 
例 15-7 ”倒立 摆 。 
考虑 如 图 15-4 所 示 的 倒立 摆 。 
其 运动 方程 为 
ml20 —mglsind =Q (15-50) 
为 了 控制 单 摆 并 且 将 其 从 初始 角 O00 带 到 垂直 向 上 的 位 置 ， 我 们 可 
以 利用 如 下 的 一 个 反馈 控制 规律 。 
Q=—kp 6 —kpO—megl sind (15-51) 
参数 kp 和 kp 是 正 增益 ,假设 其 为 常数 。 控 制 规律 式 (15-51) 
将 系统 动力 学 变换 为 
ml20 +kp 6+kpO=0 (15-52) 
式 (15-52) 表明 系统 作为 一 个 稳定 的 质量 -弹簧 -阻尼 器 而 动作 。 
假使 单 摆 的 期 望 位置 位 于 一 个 非 零 角度 ，0=04， 我 们 可 以 利用 基于 如 下 误差 的 反馈 控 
制 规律 。 



































下 
Se 
S 





图 15-4 受 控 的 倒立 摆 


























Q=ml?6,—kp e —kpe—me)l sind (15-53) 
将 该 控制 规律 代入 式 〈15-50) 中 ， 则 可 知 受 控 系 统 的 动力 学 方程 为 
ml2e +kp e +kpe=0 (15-54) 


例 15-8 2R 平面 机 械 手 的 控制 。 
2R 平面 机 械 手 如 图 15-5 所 示 ， 其 已 知 的 动态 方程 为 


图 ja D A, a | 01 f) 
= . (+ i+ (15-55) 
Q2 Da Dz), Ca C22)\0, G2 





= 








这 里 
Du=miritltmes Udi tlirzcosd2+r3) +12 (15-56) 
Dis=D21=m2lirscos0s mr? +I? (15-57) 
D22 =m5r} +I? (15-58) 
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图 15-5 具有 质量 连 杆 的 2R 平 面 机 械 手 




















Ci 一 一 mir20ysin0， (15-59) 
Co =—molir2(6, +6) sind2 (15-60) 
Ci2=me2lir26 sind, (15-61) 
Cz =0 (15-62) 
Gı =m grıcoslı tmzg (licosO1tr2cos(01t0,)) (15-63) 
G2=m2gr2cos(O; +02) (15-64) 
让 我 们 将 运动 方程 写 为 
Do)g+Cc,d)g+GC)=O (15-65) 
sk (15-65) 两 边 乘 以 也 -1， 将 运动 方程 转换 为 
q +D-'iCg +D-IG =D-'Q (15-66) 
为 了 控制 机 械 手 并 使 其 服从 期 望 路 径 g 二 qa(t)， 我 们 应 用 下 列 控 制 规律 : 
Q =D(q)U +C(q ,gq )g +G (q) (15-67) 
这 里 
U ~—gs—2ke —k2e (15-68) 
e =q qd (15-69) 


REU 是 控制 器 输入 ，e 是 位 置 误差 , & 是 一 个 正常 数 增益 。 将 控制 规律 代入 运动 方程 
可 知 误差 矢量 满足 线性 二 阶 常 微分 方程 为 
e 十 2&e +k2e 一 0 (15-70) 











因此 ， 误差 指数 性 地 收敛 于 零 。 


15.3 线性 控制 技术 











在 机 器 人 操作 点 处 应 用 线性 控制 算法 并 将 其 运动 方程 线性 化 是 一 个 古老 的 实际 机 器 人 控 
制 方法 。 这 种 技术 能 够 很 好 地 在 操作 点 附近 发 挥 作 用 。 因 此 ， 它 仅仅 是 一 个 局 部 的 稳定 方 
法 。 线 性 控制 技术 是 比例 (proportional), #4 (integral)、 微 分 (derivative) 及 其 任何 
综合 。 

这 个 思想 就 是 线性 化 一 些 参考 操作 点 的 非 线性 运动 方程 ， 形 成 一 个 线性 系统 ， 设 计 线性 
系统 控制 器 ， 然 后 将 控制 应 用 于 机 器 人 。 这 种 技术 在 操作 点 的 邻近 区 域 总 是 可 以 获得 一 个 稳 
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定 的 控制 器 。 然 而 ， 稳 定 的 邻近 区 域 可 以 是 很 小 的 ， 并 且 很 难 确定 。 
比例 -积分 -微分 控制 PID 算法 利用 位 置 误差 、 积 分 误差 和 微分 误差 构成 一 个 控制 规 
律 。 因 此 ， 对 于 输入 指令 ，PID 控制 规律 有 下 列 的 通用 形式 : 


Q= keetki | aah (15-71) 
=q-qa RES. key ki ko 分 别 是 与 比例 、 积 分 和 微分 控制 器 相关 联 的 正常 








Ling 


E 





wit. 
这 样 以 来 ， 控 制 指 令 Q 就 是 这 3 项 的 和 : 与 误差 e 成 比例 的 比例 PO H, SRA e 积 
分 成 比例 的 积分 D 项 ， 与 误差 e 微分 成 比例 的 微分 CD) 项 。 


15.3.1 比例 控制 


对 于 比例 控制 来 说 ，PID 控制 规律 式 (15-71) 可 以 简化 为 
Q=kpetQa (15-72) 
变量 Qa 是 期 望 控 制 指令 ， 该 变量 被 称 为 偏差 或 者 重 置 因子 。 当 误差 信号 为 零 时 ， 控 出 
上 § 令 等 于 期 望 值 。 比 例 控制 的 缺点 是 ， 在 稳定 状态 条 件 下 通常 会 有 一 个 常数 误差 。 


15.3.2 积分 控制 


积分 控制 的 主要 功能 是 消除 稳 态 误差 .并 且 使 系统 服从 于 稳 态 条 件 下 的 预 置 点 。 积 分 控 

制 恬 会 产生 正 误差 增加 的 控制 指令 ， 以 及 负 误 差 减少 的 控制 指令 。 积 分 控制 器 通常 与 比例 控 
制 器 一 起 使 用 。 对 于 PI 控制 器 来 说 ， 其 控制 规律 为 

Q=kpe+ kı | a (15-73) 


0 


Lae 
i 








= 























15.3.3 微分 控制 


微分 控制 的 目的 就 是 改善 系统 的 闭环 稳定 性 。 微 分 控制 絮 利 用 正切 水 数 推断 误差 曲线 。 
微分 控制 器 通常 与 比例 控制 器 一 起 使 用 。PD 控制 规律 为 
Q=kpetkpe (15-74) 





iE AA : 
如 果 任 何 线性 系统 足够 接近 参考 操作 点 ， 那 么 它 的 行为 将 是 线性 的 。 考 虑 一 个 非 线性 
系统 














q =f (gq.,0) (15-75) 
WR ga 是 由 具体 输入 Q 所 产生 的 一 个 解 ， 即 
qa=f (qa,Q.) (15-76) 
假设 6q 是 距 参 考点 qj 的 一 个 微小 改变 量 ， 因 为 微小 改变 量 OQ KAFQ. WA 
q =—=qatodg (15-77) 
Q =Q.1700 (15-78) 











如 果 假 设 在 所 有 时 间 里 都 有 微小 改变 量 6q 和 60 ， 那 么 可 以 利用 泰勒 级 数 扩 展 式 近 似 化 
式 (15-75). q 是 下 列表 达 式 的 解 。 


q 一 二 二 dg 十 二 一 O (15-79) 
9 d d 
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微分 算 阵 2 大] 和 [2 是 参考 点 qa Qa) 处 的 估计 值 。 
d d 


KB 15-9 单 摆 的 线性 控制 。 

11-12 所 示 为 一 个 受 控制 的 单 摆 ， 即 单 臂 机 械 手 。 该 机 械 臂 的 运动 方程 为 
Q=10+c¢6 +mglsind (15-80) 

DLO AY oe at, Fe AY Ba HE BEE ABO c, LAA 








这 里 , I EHR SE SCA ESE, m 是 
质心 C 之 间 的 距离 是 1。 引 入 一 组 新 变量 : 














0=x1 (15-81) 

=r (15-82) 

则 X1—=x2 (15-83) 
将 运动 方程 转换 成 

z = Q-cx2 amet sinz] ET 


这 些 方程 的 线性 化 形式 是 


a nal 
| 十 (15-85) 
£ —mgl/I —c)\a2 0 1/1) \Q 


该 形式 实质 就 是 现代 控制 理论 中 所 述 的 状态 空间 模型 。 


假设 参考 点 矢量 为 
= (*)= bi (15-86) 
£? 0 





























Q.=mgl (15-87) 
式 (15-85) 中 的 系数 矩阵 必须 在 参考 点 处 进行 评估 。 我 们 使 用 一 组 采样 数据 
m=l|kg 
/一 0. 35m 
1=0. 07kg + m? 
c=0.01N* s/m (15-88) 
并 且 求 得 
-| | (15-89) 
Oda \—49.05 —0.01 
af (0 0 
N 14. sae) E 





现在 我 们 有 一 个 线性 系数 ， 并 且 可 以 应 用 能 应 用 于 线性 系统 的 任何 控制 规律 。 例 如 ， 一 
个 PID 控制 规律 : 











` t 
Q =Q.—kp e —kpe Hk | e dt (15-91) 
0 


w 
HE 


一 宛 /2 
e =q T i | (15-92) 


T2 
sk (15-92) 可 以 控制 绕 着 参考 点 的 机 械 臂 。 
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例 15-10 PD 控制 。 
让 我 们 定义 一 个 PD 控制 规律 ， 即 . 
Q =—kpe —kpe (15-93) 
e =q 一 qd (15-94) 


将 PD fe fil MAF RAHAT EN BLA AH, BI 
Q =D(q)q +H(q ,gq )+G (q)=D(q)q +C qq +G q) (15-95) 
则 会 产生 下 列 控制 方程 : 
D(q)q +C(g ,gq gq 二 +G() 二 kb(g 一 gu) 一 [pg —qu) =0 (15-96) 
当 qs 是 一 个 与 机 器 人 具体 配置 相关 的 常数 矢量 时 ,该 控制 便 是 理想 的 控制 ， 因 此 
qa 一 0。 如果 是 这 样 ，PD 控制 器 可 以 使 配置 gu 为 全 局 稳定 。 
如 果 由 g 二 qa (ti) 确定 一 个 路 径 ， 我 们 将 修正 的 PD 控制 器 定义 成 下 列 形式 : 















































Q =D(q)qatC(q ,gq )qa +G (q )—kpe 一 [pe (15-97) 
将 闭环 方程 简化 为 
D(q)e +(C(q ,q )+kple +kp e 一 0 (15-98) 
控制 点 g 一 gd 三 常数 的 方程 线性 化 ， 可 以 提供 一 个 误差 信号 的 稳定 动力 学 方程 ， 即 
D(qj)e +kype +kp e =0 (15-99) 


15.4 传 感 与 控制 








位 置 传感器 、 速 率 传感器 、 加 速度 传感器 、 力 传感器 是 用 于 机 器 人 中 的 最 常用 传 感 髓 。 
考虑 图 15-6 所 示 的 倒立 摆 ， 该 倒立 摆 作 为 单 自由 度 机 械 手 ， 运 动 方程 为 
ml26 —c Ô —mglsind=Q (15-100) 
从 开 环 控制 角度 ,我们 需要 提供 一 个 转 矩 RQ G 以 驱动 机 械 手 ， 使 其 服从 运动 04 (2) 
的 期 望 路 径 ， 即 


























Q.=ml’6 , cb, mglsinĝa (15-101) 
在 机 器 人 学 中 ， 我 们 通常 利用 动力 学 方程 计算 转 矩 Q。， 并 且 用 转 矩 Q。 控制 执行 器 。 
机 械 手 响应 所 应 用 的 转 矩 后 将 会 运动 。 运 动 方程 ， 式 (15-100) 是 
一 个 实际 机 械 手 的 模型 。 换 名 话说， 我 们 要 求 机 械 手 基于 这 个 方程 进行 
工作 。 然 而 ， 有 很 多 未 建 模 的 现象 以 至 于 我 们 不 能 将 其 包括 在 运动 方程 
中 ， 或 者 不 能 对 它们 建 模 。 一 些 是 温度 、 气 压 、 确 切 的 重力 加 速度 ， 甚 
至 物理 参数 ， 例 如 我 们 认为 的 m Al L 都 要 有 较 高 的 精度 ， 因 此 应 用 控制 
指令 Q. 将 使 机 械 手 运动 ， 并 且 提 供 9、6 、V 的 真实 值 ， 这 些 值 不 必 等 
于 04、04、V4。 现 在 传 感 是 很 重要 的 ， 因 为 我 们 需要 测量 实际 的 角度 0、 
角速度 9 和 角 加 速度 ， 以 便 将 其 与 bu、64、 的 相 比较 ， 从 而 确保 机 械 
手 沿 着 期 望 的 路 径 运动 。 这 就 是 为 什么 引入 反馈 控制 系统 和 误差 信号 ce 一 
9 一 04 的 原因 。 
假设 机 器 人 在 一 个 环境 中 要 完成 一 项 工作 ， 因 此 机 器 人 可 以 与 环境 相互 作用 。 从 而 ， 机 
器 人 需要 两 种 类 型 传感器 : 四 感 测 机 器 人 内 部 参数 ， 被 称 为 内 部 传感器 ，@ 四 感 测 机 器 人 环境 
参数 ， 被 称 为 外 部 传感器 。 最 重要 的 内 部 参数 有 位 置 、 速 度 、 加 速度 、 力 、 力 和 矩 和 惯性 。 





















































图 15-6 一 个 
倒立 的 单 摆 
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15.4.1 位 置 传感器 


1) 旋转 编码 器 (Rotary encoders) 。 在 机 器 人 学 中 ， 几 乎 所 有 类 型 的 执行 器 都 提供 一 个 
旋转 运动 。 这 时 我 们 可 以 通过 齿轮 为 旋转 关节 提供 一 个 旋转 运动 ， 或 者 为 棱柱 关节 提供 一 个 
平移 运动 。 因 此 ， 理 想 的 可 能 是 检测 通过 基于 执行 器 角 位 置 的 关节 连接 的 连 杆 的 相对 位 置 。 
位 置 检测 中 的 误差 是 由 于 非 刚性 和 间隙 所 造成 的 。 最 常用 的 位 置 传感器 是 旋转 编码 器 ， 它 可 
以 是 光学 的 、 电 磁 的 和 电气 的 。 例 如 ， 当 编码 器 轴 旋 转 时 ， 计 线 的 码 盘 遮挡 光束 。 光 电 探 测 
器 将 光电 脉冲 转换 成 一 个 可 计数 的 二 进 制 波形 。 这 时 轴 角 由 于 所 统计 的 脉冲 数目 所 确定 。 

2) 旋转 变压器 (Resolvers)。 我 们 可 以 设计 一 个 电子 装置 以 提供 一 个 关节 变量 的 数学 
函数 。 数 学 函数 可 以 是 正弦 、 余 弦 、 指 数 或 者 任何 综合 的 数学 画 数 。 这 时 求解 数学 函数 可 以 
间接 计算 关节 变量 。 正 弦 和 余弦 函数 是 比较 常用 的 函数 。 

3) 电位 计 (Potentiometers)。 利 用 电 桥 ， 电 位 计 可 以 提供 一 个 与 关节 位 置 成 比例 的 
电压 。 

4) LVDT 和 RVDT。LVDT/RVDT 就 是 线性 /旋转 变量 微分 变换 器 ， 操 作 具 有 共同 
磁铁 心 的 变压器 。 当 铁心 移动 时 ， 一 个 变压器 的 输出 增 大 ， 与 此 同时 另 一 个 变压器 的 输出 则 
减 小 。 电 流 的 差额 就 是 对 铁心 位 置 的 一 个 测量 。 


15.4.2 速度 传感器 


1) 转速 计 (Tachometers)。 一 般 来 说 ,转速 计 对 于 任何 速度 传感器 来 说 就 是 一 个 名 
词 。 转速 计 通 常 提供 一 个 与 转轴 角速度 成 比例 的 模拟 信号 。 利 用 不 同 的 物理 特征 ， 如 电磁 
场 ， 有 大 量 的 不 同 设计 。 

2) 旋转 编码 器 (Rotary encoders) 。 任 何 旋转 传感器 都 可 以 配备 时 间 测 量 系 统 ， 然 后 就 
变 成 了 角速度 传感器 。 编 码 器 计数 旋转 码 盘 的 光电 脉冲 数 ， 这 时 角速度 则 由 脉冲 之 间 的 时 间 
决定 。 

3) 微分 装置 (Differentiating devices) 。 任 何 类 型 的 位 置 传感器 都 可 以 配备 数字 微分 装 
置 以 变 成 一 个 速度 传感器 。 数 字 或 数字 化 的 微分 装置 需要 一 个 简单 的 处 理 需 。 数 字 微 分 通常 
就 是 一 个 误差 处 理 过 程 。 

4) 积分 装置 (Integrating devices) 。 数 字 化 积分 加 速度 计 的 输出 信号 ， 提 出 一 个 速度 
信号 。 数 字 积 分 装置 也 需要 一 个 简单 的 处 理 器 。 数 字 微 分 通常 就 是 一 个 平稳 光滑 过 程 。 


15.4.3 加 速度 传感器 


加 速度 传感器 是 基于 牛顿 第 二 运动 定律 而 工作 的 。 该 类 传感器 传 感 能 够 使 一 个 已 知 质 
产生 一 个 加 速度 的 力 。 有 许多 类 型 加 速度 传感器 ， 应 力 -应 变 仪 、 电 容 式 加 速度 传 感 铝 、 
感 式 加 速度 传感器 、 压 电 式 加 速度 传 感 顺 、 微 加 速度 是 最 常用 的 。 在 这 些 类 型 的 传感器 中 ， 
力 能 够 在 弹性 材料 中 产生 一 个 比例 位 移 ， 如 微 悬 臂 梁 中 的 俩 斜 量 和 与 加 速度 成 比例 的 位 
移 等 。 

加 速度 计 的 应 用 包括 加 速度 测量 、 角 加 速度 测量 、 速 度 测 量 、 位 置 测量 、 角 速度 测量 、 
频率 测量 、 脉 冲 测量 、 力 测量 、 倾 斜 测 量 、 方 向 测量 。 

1) FANE REE RAE (Force and Torque Sensors)。 在 传 感 加 速度 中 我 们 所 使 用 的 任何 
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概念 和 方法 也 可 以 用 于 力 和 转 抢 的 传 感 。 我 们 用 至 少 3 个 力 传 感 器 装备 在 机 器 人 手腕 上 ， 以 
测量 与 环境 之 间 的 接触 力 和 力矩 。 当 机 器 人 接触 到 未 知 的 表面 或 者 物体 时 ， 腕 力 传感器 将 是 
地 别 重要 的 。 

2) 接近 传感器 (Proximity Sensors) 。 在 与 物体 、 场 、 特 殊 材 料 接触 之 前 ， 利 用 接近 传 
感 咒 检测 它们 的 存在 。 电 感 式 、 电 容 式 、 霍 尔 效应 、 声 波 、 超 声波 、 光 学 式 都 是 最 常用 的 接 
近 传 感 器 。 

依据 电感 变化 ， 电 感 式 传感器 可 以 检测 金属 物体 的 存在 。 电 容 式 传感器 可 以 检测 能 够 引 
起 电容 变化 的 气体 、 液 体 或 者 金属 。 霍 尔 效 应 传感器 是 基于 半导体 材料 中 的 电压 与 电磁 场 之 
间 的 相互 作用 而 工作 的 。 这 些 传 感 器 可 以 检测 磁场 和 材料 的 存在 。 声 波 、 超 声波 、 光 学 传 感 
器 则 基于 由 物体 产生 的 发 射 信号 反射 或 者 折射 而 工作 。 










































































15.5 本章 小 结 





在 开 环 控制 中 ， 对 于 给 定 的 关节 路 径 gu 王 9g (2)， 我 们 基于 下 列 运动 方程 计算 机 器 人 所 
BER AJ FRKE 











Q.=D(qa)qa HH (qasqa) HG (qa) (15-102) 
然而 ， 在 实际 关节 变量 和 期 望 值 之 间 有 一 个 偏差 。 该 偏差 被 称 为 误差 e : 
e =q 一 ga (15-103) 
e 一 9 一 ga (15-104) 
通过 测量 误差 指令 ， 我 们 可 以 定义 控制 规律 ， 计 算 新 的 控制 指令 矢量 以 补偿 误差 。 
Q =Q.+kpe +kpe (15-105) 


这 里 ， 参 数 kp 和 kp 是 常数 增益 对 角 线 矩阵 。 
控制 规律 将 实际 关节 变量 q.q) 与 期 望 值 (a qa) 相 比较 ,成 比例 地 产生 一 个 指 
令 。 应 用 新 的 控制 指令 可 以 将 机 器 人 动力 学 方程 调整 为 
Q.tkpe +kpe =D(q)q +H(g ,gq )+G (gq) (15-106) 
这 就 是 闭环 控制 ， 在 该 控制 中 ， 基 于 实际 变量 和 期 望 变 量 之 间 的 偏差 计算 控制 指令 。 所 
计算 的 控制 转 矩 为 


























Q =D(q)(qa—kpe —kpe )+H(q ,gq )+G 4q) (15-107) 
它 就 是 一 个 在 机 器 人 学 中 所 应 用 的 闭环 控制 规律 ， 用 以 使 机 器 人 服从 期 望 路 径 。 




















习题 


15-1 2 阶 系统 响应 。 

如 果 系 统 满足 xz(0)==1 和 x (0) 二 0,， 求解 特征 方程 ， 并 且 确 定 下 列 2 阶 系统 在 x(1) 处 
的 响应 。 

1) 并 十 2 之 十 5z 一 0。 

2) 过 十 2 之 十 过 一 0。 

3) a+42+2=0, 

15-2 ”修正 的 PD 控制 。 























= 
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将 修正 的 PD 控制 规律 
f=—kpe—kax 
e =r Td 
应 用 于 2 阶 线性 系统 
mx 十 CX 十 kx =f 
将 系统 简化 为 误差 信号 的 2 阶 方程 。 
me 十 (c 十 AD)e 十 (& 十 &p)e 一 AzD 
这 时 ， 计 算 阶 跃 输 入 的 稳 态 误差 。 
二 Xd 二 和 常数 





15-3 ”修正 的 PID 控制 。 
将 修正 的 PID 控制 规律 


t 
f=—kpe— kax ti Í edt e=x-2Xa 
0 





应 用 于 2 阶 线性 系统 
max ter the = f 
将 系统 简化 为 一 个 误差 信号 的 3 阶 方程 。 
me 十 (c 二 kp)e 十 (k 十 kp)e 十 Rie 一 0 
这 时 ， 试 求 PID 增益 ， 以 便 系 统 的 特征 方程 简化 为 
AZ +H2éwn,à Fw) TB)=0 




















15-4 线性 化 。 
线性 化 给 定 的 方程 ， 并 且 确 定 线性 化 方程 组 的 稳定 性 。 
Tı =x +r coszr? 
T =T? (1 十 Xl1 十 X2)X1 十 X1sinx2 








15-5 ”利用 下 列 控制 规律 扩展 2R 平面 机 械 手 的 控制 方程 。 
Q =D(q )(qa—kve —kpe )+H(q q )+G (q ) 

15-6 单 杆 机 械 手 控制 。 

单 杆 机 械 手 如 图 15-6 所 示 。 

1) 推导 运动 方程 。 

2) 确定 在 0(0) 王 45" 和 0(0) 王 一 45" 之 间 的 从 项 止 到 静止 的 关节 路 径 。 

3) 求解 机 械 手 的 时 间 最 优 控 制 ， 并 且 依 据 下 列 参数 确定 转 矩 Qe(z) 。 

m=lkg l=lm |Q|<120N+m 

4) 现 假设 质量 mm 二 1.01kg， 利 用 进 给 计算 转 矩 Q.(1) 数值 化 求解 运动 方程 。 确 定 运 动 
终点 处 的 位 置 误差 和 速度 误差 。 

5) 设计 一 个 计算 转 矩 控制 规律 以 补偿 运动 期 间 的 误差 。 

Weil5-7 质量 弹簧 控制 。 

求解 习题 14-10， 计 算 最 优 控制 输入 。 增 加 刚度 10% ， 设 计 一 计算 转 矩 以 消除 运动 期 间 
的 误差 。 

W15-8 2R 机 械 手 控制 。 

1) 求解 习题 13-20， 计 算 最 优 控制 输入 。 
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2) 质量 增加 了 10%， 数 值 化 求解 动态 方程 。 

3) 应 用 计算 的 最 优 输入 ,确定 笛 卡 儿 空 间 和 关节 空间 中 的 位 置 误差 和 速度 误差 。 

4) 设计 一 计算 转 矩 控制 规律 ， 以 消除 运动 期 间 的 误差 。 

友 15-9 PR 平面 机 械 手 控制 。 

1) 求解 习题 14-13， 计算 最 优 控 制 输入 。 

2) 重力 加 速度 增加 了 10%， 数 值 化 求解 动态 方程 。 

3) 应 用 计算 的 最 优 输入 ,确定 笛 卡 儿 空 间 和 关节 空间 中 的 位 置 误差 和 速度 误差 。 

A) 设计 一 计算 转 矩 控制 规律 以 消除 运动 期 间 的 误差 。 

15-10 传 感 和 测量 

考虑 图 5-6 中 所 示 的 单 自由 度 机 械 手 。 为 了 控制 机 械 手 ， 我 们 需要 检测 实际 角 0、 角 速 
度 6 和 和 角 加 速度 六 ， 将 其 与 04、64、V 4 比较 以 确保 机 械 手 遵从 所 期 望 的 路 径 。 我 们 可 以 确 
定 由 执行 器 所 提供 的 实际 力矩 Q? 可 以 用 替换 的 期 望 值 Q. 进行 比较 吗 ? 使 Q 等 于 Q。 可 以 
保证 机 械 手 执行 所 给 的 假设 吗 ? 
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附录 A 全 局 坐标 系 三 重 旋转 


在 这 个 附录 中 将 给 出 绕 着 全 局 固定 轴 三 重 旋转 的 12 种 组 合 。 








cacB — cBsa sp 
Qx.yQy.pQz.c=|cysatcasBsy cacy—sasßpsy —cBsy (A-1) 
sY¥sa—casBcy casyt+sasBcY  cBcy 
cyc sasy—casBcey casy + sasßcy 
ede SB ca ch — sach (A-2) 
一 SYc8 sacy+casßsy cacy— sasBsy 
cacy—sasBsy —cpsy casy + sasßpcy 
Q7z.x¥Qx pQy.a = ° sytsasBcy  cBcY  sasy—casBcy (A-3) 
一 cpsa sp ca ch 
cBcY —casy+sasBcY sasy caspey 
Qz,ryQr,pQx,v 王 区 cacy +sasBsy  sasy+casBsy (A-4) 
— sg se cp ca ch 





cacy +sasBsy —sacy+casßsy cBsy 
Qy.7~Qx.pQz.0 -| sa cB ca cB 一 SB (A-5) 
casy+sasBcY  sasy¥yt+casBcy cBey 
ca cB — sg sa ch 
Qx. QZ. BQY, a = | sa sy + ca sp cy cBcy — casy +saspcy (A-6) 
on cBsY cacy sasBsy 
sa s8 ca sB 
Qx.yQy.pQx.n | # sBsY  cacY—sacBsY —sacY—cacBsy (A-7) 
—cysB casy+sacBcy —sasy+cacBcy 
—sasy+cacBcy 一 S8cy casy +sacpcy 
Qy,7yQz.pQy,a -| ca cB cB sasp (A-8) 
—sacy—cacpsy  sBsY cacy— sacBsy 
cacy—sacBsY —sacY—cacBsY sasy 
Q7.7Qx.pQz.0= corm —sasyt+cacBcy —sBcy (A-9) 
sa s8 ca sB cB 
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Qx. 7QZ. QX. = 


Qy. yQ x. pQY.a = 


QZ. yQY. BQZ. a = 


附录 


cp —casp sasp 
sßĝcy 一 SasyY 十 cacpcy 
SBSY  sacy + cacpsy 


— casy —sacpcy 


cacy — sa cpsy 


cacy—sacBsY  sBsY  sacY+cacBcy 
sa sf cp — cash 
—casy—sacpcy cBcY —sasy+cacpcy 
—sasyt+cacBcy —casy—sacBcyY sßey 





sa cY + cacBsy cacy—sacBsy sBsY 


— cash sa sf cB 


B 局 部 坐标 系 三 重 旋转 


在 这 个 附录 中 将 给 出 绕 着 局 部 固定 轴 三 重 旋转 的 12 种 组 合 。 


AryAy,0A:,y = 


Ayy 2 0Axr.¢ = 


AzgAr0A yP == 





AzwAyo0Ax,¢ E 


Ay gAxzoAz.e — 


AxryAz0Ay.¢ = 


Az, yA y, Arzo = 


Ay yAz pA yp = 





Az, yA zx, oA z:o = 
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c0 co c0 so 一 SO 

一 cWsP 十 SWsOcPp cycptsysOsp spc 

SWsD 十 cWsOcp ”一 SWcp 十 cWsOsSP cyWcO 

coOcy syspteysocp 一 SWcp 十 cWSOSD 
一 SO co ce co sp 


cOsy 一 cWSP 十 SWSsOcp 
cWcp 十 SWSOSP spcð 一 cWSP 十 SWSOcp 
一 SWcp 十 cWsOsP cy spse +tcyslco 
cOSP 一 SO cOcp 


cyWcp 十 SWSOSO 











cyc) = SWcp 十 cWSOSP  SWSPp 一 cWsOcp 
一 SWc0O cycp—sysOsp  cWSP 十 SWSOcp 
SO 一 cOsp cOcop 
cWcp 一 SWSOSP  cWsSp 十 SWSsOcp 一 SWcO 
一 cOsp cOcp SO 
SWcp 十 cWsOsPp  SWSsP 一 cWSsOcp cpc 
cOcp SO 一 cOSP 
SWSD 一 cWSOcp cy spego tceypshso 
cWSP 十 SWSOcp ”一 SWc0O  cWcp 一 SWSOSP 
cO sso 一 SOcp 
sys? cycp— svcd sep cys + spcdc@ 
cys) 一 SWcp 一 cWcgsp 一 SWSD 十 cWcOcp 


一 SWSP 十 cWcOcp cos) 一 SWcp 一 cWcOSp 
一 SOcp cO SOSD 
cWSD 十 SWcgcp sys0 cWcp 一 SWcOSp 
cWcop 一 SWcOSD cWSD 十 SWcOcp  swsd 
一 SWcp 一 cWcOsp 一 SWSD 十 cWcOcp cwsd 

SOSD 一 cpSO cO 











CA-10) 


(A-11) 


(A-12) 


(B-1) 


(B-2) 


(B-3) 


(B-4) 


(B-5) 


(B-6) 


(B-7) 


(B-8) 


(B-9) 


ne Ci 
































cO SOco sO sp 
Ax gAz0Axr,¢=| 一 cWSO SWSP 十 cWcOcp  SWcp 十 cWcOsp (B-10) 
SWSO —cdsp—scbcp cyWcp 一 SWcOsp 
cWcp 一 SWcOsP sys0 一 cWSD 一 SWcOcop 
Ay gAx0Ayo= sO s@ ey slep (B-11) 
speco + cych se cy s0 sysp + ceychco 
一 SWSP 十 cWcOcp sycptcecycOsp 一 cWSO 
AzwAy~0Az.e=|—cpyse—cpcbse  cWcp 一 SWcOSP sys0 (B-12) 
sO cp SOSD cO 
附录 C 中 心 螺旋 运动 的 三 重组 合 旋转 
在 这 个 附录 中 将 给 出 三 重 中 心 螺旋 运动 的 6 种 组 合 
cacB — sach sp Ypx tapzsb 
FO ee = sacytcasBsy cacy—sasBsy —cBsy BpycY—apzcBsy 
sasy¥—casBcy casytsasBcy cBcy BbysY+apzcBcy 
0 0 0 1 
(C-1) 
cac8 —sPsY—sacBcy sBcY+sachsy apzs8+Ypxcacf 
ity PD haza R hx D=| “ an ee 
casB cBsy+sasBcy  cBcY—sasBsy apzc8—YpxcasP 
0 0 0 1 
(C-2) 
cac8—sasBsy —sacy casB+sacBsy Ypxca—Bpysacy 
E E E sacB+casBsyY cacy casB—cacBsy YpxsatBpycacy 
—sBcy sy cBey apz +PBpyYsy 
0 0 0 1 
(C-3) 
cac8 —sacY-casBsy sasy+caspey  —Bpysat+YpxcacB 
TE a hy Bb hx yb = sac  cacy+sasBsy —casy+sasBcy BpycatYpxsacf 
—sB cBsy cBey apz—7Ypxsp 
0 0 0 1 
(C-4) 
cacBt+sasBsy —sacB-+casBsy ey YpxcBtapzcBcy 
ee ee a= seed poe coe ee he 
—casBt+sacBsy  sasBt+cacBsy cBcY apzcBcY—ypxsB 
0 0 0 1 
(C-5) 
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acp 一 Sa 
$Chx sy Ts hz.a Ks (hy Bo J)= 


0 0 


附录 D 三 角 曙 数 公式 


1) 基于 指数 的 三 角 函 数 定义 : 


gF +e iz 
cosz = 7 
. e=—e 过 
sinz = 7i 
e`? —e 这 
tanz S Cer Fee) 
e} =cosz +isinz 
e® =cosz —isinz 


2) 两 角 和 与 差 的 三 角 函 数 公式 : 


sin(a +f) =sinacos$ + cosa sinf 





cos(a +f) = cosa cosp + sina sing 





tana = tang 


tana rp) = 





1+ tana tang 
py CcotacotB 十 1 
EEE cota + cotf 
3) 三 角 函 数 的 对 称 性 : 
sin(—a)=—sina 


cos(—a) =cosa 


tan(—a)=~—tana 





4) 三 角 函 数 的 倍 角 公式 : 
2tana 


sin(2a) =2sina cosa = Tan 





cos(2a)=2cos?a —1=1—2sin?a=cos*a 


2t 
tan(2a) = 
1—tan‘a 
ta—l 
LoS 
2cota 
sin(3a) =—4sin°a+3sina 


cos(3a ) = 4cos? a — 3cosa 


一 tan3w 十 3tana 
一 3tan2w 十 1 





tan(3a ) 一 
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ag 
sBsyt+sacBcy cacy 一 99SY 十 saxs9cy 
—sBcy+sacBsy casy cBcy+sasBsy 


sin’a 


Yp x —Ppy sa 
—ap z sY HBpy cacy 
ap z cY +Bpycasy 


(C-6) 


(D-1) 


(D-2) 


(D-3) 
(D-4) 
(D-5) 
(D-6) 
(D-7) 


(D-8) 


(D-9) 


(D-10) 
(D-11) 
(D-12) 


(D-13) 


(D-14) 


(D-15) 


(D-16) 


(D-17) 
(D-18) 


(D-19) 


ne Ci 





sin(4a) = — 8sin® a cosa + 4sina cosa 
cos(4a)=8costa—8cos?a +1 


—4tan?a+4tane 
tanta —6tan?a +1 


sin(Sa) =16sin”a —20sin?a +5sina 

cos(5a) = 16cos’a — 20cos*a 十 5cosa 
sin(na) =2sin| (n—1)a Jcosa—sin[ (1 —2)a | 
cos(na) =2cos|. (2 —1)a |cosa —cos| (1 —2)a | 


tan[ (2 —1)a ]+ tana 
1—tan[ (n—1)a |tane 


1 十 cos 
cos( 2] = | a 
] 一 
sin($ |]=+ | a 
; a \ 1l—cosa__ sina ea ii /1—cosa 
sa 2 sina 1 十 cosa ` N 1l+cosa 
2tan{ $) 








tan(4a) = 





tan(na) = 


5) 三 角 函 数 的 半角 公式 : 











2 
sina 一 
Ï 2 os 
十 tan ($) 
1—t (2) 
an k 
cosa = 一 一 一 一 
1 十 tan2| < 
an? [4] 


6) = Ff KR FF 
1 
sin’a => L1—cos(2a) | 
f i. 
sina cosa = pg sin(Za) 
1 
costa => [1+ cos(2a) | 
: 1 . 
sinë a = 7 Lsina —sin(3a) | 


1 
acosa =~ [cosa — 3cos(3a) | 


sin 
i Le : 
sina cos? a = 本 [sina +sin(3a) ] 


1 
cos3a = qLeos(3a ) 十 3cosa ] 


1 
sin'a =- 13—4cos(2a) + cos(4a) ] 


(D-20) 
(D-21) 


(D-22) 


(D-23) 
(D-24) 
(D-25) 
(D-26) 


(D-27) 


(D-28) 


(D-29) 


(D-30) 


(D-31) 


(D-32) 


(D-33) 


(D-34) 


(D-35) 


(D-36) 


(D-37) 


(D-38) 


(D-39) 


(D-40) 
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1 
sin’acosa =-~ | 2sin(2a) —sin4a) ] (D-41) 
2 2 == 1 
sin”acos’a =| 1— cos(a) ] (D-42) 
1 
sinacos’a =-~ | 2sin(2a) + sin(4a) ] (D-43) 
1 
costa =- [3 F4cos(2a ) + cos(4a) | (D-44) 
1 
sin’a = JoL 10sina —5sin (3a ) + sin(Se) | (D-45) 
1 
sint acosa = Fo L2cosa — 3cos(3a) + cos(Sa) | (D-46) 
1 
sin” acos”a = Tol 2sina +sin(3a)— sin Sa) ] (D-47) 
sinzecosia 一 下 [2cose 一 3cos(3c) 一 5cos(5c)] (D-48) 
1 
sinacos'a =l 2sina + 3sin(3a) + sina) ] (D-49) 
1 
cos’a = Fpl 10cosa + 5cos(3a) +cos(5a) ] (D-50) 
1—cos(2a) 
a _ 
tana TF cos(2a) a 
7) 正弦 和 余弦 之 积 的 三 角 函 数 公 式 : 
cosa cose = ; cos(a—B)+ Fcos(e FB) (D-52) 
oe 1 1 
sina sing = z costa p) z costa p) (D-53) 
l l, i. 
sina cos8 = -ysin Ca Boo 7 sina B) (D-54) 
l 1. I 
cosa sinf = 2 sin(a +f) 3 sin(a—f) (D-55) 
sin(a +f) sin(a — 2) =sin?a —sin?B=cos?B—cos’a (D-56) 
cos(a +8)cos(a — f) = cos? p+ sin? a (D-57) 
8) AAWE KAAR: 
+ + 
sina + sing =2sin = Ecos Ë (D-58) 
cosa + cos =2cos = Feos 2 sf (D-59) 
cosa — cosp = asin oP sin SP (D-60) 
mene 
oie E (D-61) 
cosa cos 
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ne Ci 




















sin(Bxa) 
cota + corp = Se Pe 
sina sinf 
en p 
| | an 
sine+ sing _ 2 
sina — sinf ax 一 有 
tan —— 
2 
sina + sing —atB 
=cot 
cosa — cosB 2 
sina+sinB = at 
cosa t cosp 2 
sina — sinf ax 一 有 
=tan 
cosa F cosp 2 





9) 三 角 关 系 : 
sin?a — sin?B=sin(a + f)sin(a—f) 


cos?a — cos? B=—sin(a +8) sin(a—f) 





(D-62) 


(D-63) 


(D-64) 


(D-65) 


(D-66) 


(D-67) 
(D-68) 
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本 书 详细 地 介绍 了 机 器 人 学 在 实际 中 的 应 用 ， 提 供 了 现实 应 用 中 的 相关 理 
论 和 规范 性 证 明 。 书 中 融入 了 作者 Reza N.Jazar 的 实用 经 验 、 实 例 及 说 明 : 












> 300 多 个 详细 上 且 具 有 完全 解 的 实例 。 

> 对 于 具有 执行 器 、 传 感 器 和 控制 器 的 机 器 人 系统 ， 提 供 了 有 关 其 零 部 件 和 
机 构 的 全 新 范围 。 

> 运动 学 和 动力 学 的 扩充 材料 。 

> A mie. AR. En eiA EMM. 


本 书 有 应 用 工程 师 、 研 究 人 员 和 学 生 比较 喜欢 的 众多 有 关机 器 人 学 主题 、 
最 经 典 方向 、 速 度 和 正 向 运动 学 的 系统 介绍 。 


